From 0c05dab08241af7e4c2695a600e323b1eefd1166 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 28 Oct 2021 00:52:57 -0400 Subject: [PATCH 01/28] Add Foxy support Signed-off-by: Abrar Rahman Protyasha Going from `SerializedPublisher` to generic Signed-off-by: Abrar Rahman Protyasha Migrate from rclcpp generic pub/sub With this patch, the package builds successfully in rolling, but there are failing tests to address, and all of this is before having built in foxy at all (and before adding back some of the `create_subscription` code from PR 24). Signed-off-by: Abrar Rahman Protyasha Add missing compress/decompress logic Signed-off-by: Abrar Rahman Protyasha Update generic pub sub source Signed-off-by: Abrar Rahman Protyasha Publish the message rather than its shared ptr Signed-off-by: Abrar Rahman Protyasha Rename connext foxy package Signed-off-by: Abrar Rahman Protyasha Address uncrustify error Signed-off-by: Abrar Rahman Protyasha Migrate back to rosidl_target_interfaces Signed-off-by: Abrar Rahman Protyasha --- CMakeLists.txt | 2 + package.xml | 2 +- src/domain_bridge/domain_bridge.cpp | 125 ++++++++++----------- 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, 378 insertions(+), 68 deletions(-) create mode 100644 src/domain_bridge/generic_publisher.cpp create mode 100644 src/domain_bridge/generic_publisher.hpp create mode 100644 src/domain_bridge/generic_subscription.cpp create mode 100644 src/domain_bridge/generic_subscription.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d6f850b..2588b06 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,8 @@ add_library(${PROJECT_NAME}_lib SHARED src/${PROJECT_NAME}/compress_messages.cpp 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}/service_bridge_options.cpp diff --git a/package.xml b/package.xml index d9fd5bd..853fdca 100644 --- a/package.xml +++ b/package.xml @@ -30,7 +30,7 @@ test_msgs - rmw_connextdds + rmw_connext_cpp rmw_cyclonedds_cpp rmw_fastrtps_cpp diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 86886ae..991e2fa 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -30,8 +30,6 @@ #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 "rclcpp/serialization.hpp" #include "rosbag2_cpp/typesupport_helpers.hpp" @@ -44,43 +42,24 @@ #include "domain_bridge/topic_bridge_options.hpp" #include "domain_bridge/msg/compressed_msg.hpp" +#include "generic_publisher.hpp" +#include "generic_subscription.hpp" #include "wait_for_graph_events.hpp" namespace domain_bridge { -/// \internal -/** - * A hack, because PublisherBase doesn't support publishing serialized messages and because - * GenericPublisher cannot be created with a typesupport handle :/ - */ -class SerializedPublisher +[[noreturn]] static void unreachable() { -public: - template - explicit SerializedPublisher(std::shared_ptr> impl) - : impl_(std::move(impl)), - publish_method_pointer_(static_cast( - &rclcpp::Publisher::publish)) - {} - - explicit SerializedPublisher(std::shared_ptr impl) - : impl_(std::move(impl)), - publish_method_pointer_(static_cast( - &rclcpp::GenericPublisher::publish)) - {} - - void publish(const rclcpp::SerializedMessage & message) - { - ((*impl_).*publish_method_pointer_)(message); // this is a bit horrible, but it works ... - } - -private: - std::shared_ptr impl_; - using PointerToMemberMethod = - void (rclcpp::PublisherBase::*)(const rclcpp::SerializedMessage & message); - PointerToMemberMethod publish_method_pointer_; -}; +#if defined(__has_builtin) +#if __has_builtin(__builtin_unreachable) + __builtin_unreachable(); +#endif +#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) + __builtin_unreachable(); +#endif + throw std::logic_error("This code should be unreachable."); +} /// Implementation of \ref DomainBridge. class DomainBridgeImpl @@ -90,8 +69,8 @@ class DomainBridgeImpl using TopicBridgeMap = std::map< TopicBridge, std::pair< - std::shared_ptr, - std::shared_ptr>>; + std::shared_ptr, + std::shared_ptr>>; using ServiceBridgeMap = std::map< detail::ServiceBridge, std::pair, std::shared_ptr>>; @@ -195,32 +174,39 @@ class DomainBridgeImpl type, "rosidl_typesupport_cpp"); } - std::shared_ptr + std::shared_ptr create_publisher( rclcpp::Node::SharedPtr node, const std::string & topic_name, - const std::string & type, const rclcpp::QoS & qos, - rclcpp::PublisherOptionsWithAllocator> & options) + const rosidl_message_type_support_t & typesupport_handle, + rclcpp::CallbackGroup::SharedPtr group) { - std::shared_ptr publisher; if (options_.mode() != DomainBridgeOptions::Mode::Compress) { - publisher = std::make_shared( - node->create_generic_publisher(topic_name, type, qos, options)); - } else { - publisher = std::make_shared( - node->create_publisher(topic_name, qos, options)); + auto publisher = std::make_shared( + node->get_node_base_interface().get(), + typesupport_handle, + topic_name, + qos); + node->get_node_topics_interface()->add_publisher(publisher, std::move(group)); + return publisher; } + auto publisher = std::make_shared( + node->get_node_base_interface().get(), + *rosidl_typesupport_cpp::get_message_type_support_handle(), + topic_name, + 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, - rclcpp::SubscriptionOptionsWithAllocator> & options) + const rosidl_message_type_support_t & typesupport_handle, + rclcpp::CallbackGroup::SharedPtr group) { std::function)> callback; switch (options_.mode()) { @@ -236,7 +222,7 @@ class DomainBridgeImpl compressed_msg.data = domain_bridge::compress_message(cctx, std::move(*msg)); rclcpp::SerializedMessage serialized_compressed_msg; serializer.serialize_message(&compressed_msg, &serialized_compressed_msg); - publisher->publish(serialized_compressed_msg); + publisher->publish(serialized_compressed_msg.get_rcl_serialized_message()); }; break; case DomainBridgeOptions::Mode::Decompress: @@ -251,31 +237,39 @@ class DomainBridgeImpl serializer.deserialize_message(serialized_compressed_msg.get(), &compressed_msg); rclcpp::SerializedMessage msg = domain_bridge::decompress_message( dctx, std::move(compressed_msg.data)); - publisher->publish(msg); + publisher->publish(msg.get_rcl_serialized_message()); }; break; default: // fallthrough case DomainBridgeOptions::Mode::Normal: callback = [publisher](std::shared_ptr msg) { // Publish message into the other domain - publisher->publish(*msg); + publisher->publish(msg->get_rcl_serialized_message()); }; break; } if (options_.mode() != DomainBridgeOptions::Mode::Decompress) { // Create subscription - return node->create_generic_subscription( + auto subscription = std::make_shared( + node->get_node_base_interface().get(), + typesupport_handle, topic_name, - type, qos, - callback, - options); + callback); + node->get_node_topics_interface()->add_subscription(subscription, std::move(group)); + return subscription; } - return node->create_subscription( + auto subscription = std::make_shared( + node->get_node_base_interface().get(), + *rosidl_typesupport_cpp::get_message_type_support_handle(), topic_name, qos, - callback, - options); + [publisher](std::shared_ptr msg) { + // Publish message into the other domain + publisher->publish(msg->get_rcl_serialized_message()); + }); + node->get_node_topics_interface()->add_subscription(subscription, std::move(group)); + return subscription; } void bridge_topic( @@ -378,29 +372,26 @@ 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(); + 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, - publisher_options); + *typesupport_handle, + topic_options.callback_group()); // Create subscription for the 'from_domain' auto subscription = this->create_subscription( from_domain_node, publisher, topic, - type, qos, - subscription_options); + *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 new file mode 100644 index 0000000..ad55a81 --- /dev/null +++ b/src/domain_bridge/generic_publisher.cpp @@ -0,0 +1,55 @@ +// 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(const rmw_serialized_message_t & message) +{ + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), &message, nullptr); + + 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 new file mode 100644 index 0000000..9a5d85f --- /dev/null +++ b/src/domain_bridge/generic_publisher.hpp @@ -0,0 +1,46 @@ +// 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(const rmw_serialized_message_t & 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 new file mode 100644 index 0000000..55caaaf --- /dev/null +++ b/src/domain_bridge/generic_subscription.cpp @@ -0,0 +1,115 @@ +// 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 new file mode 100644 index 0000000..a293777 --- /dev/null +++ b/src/domain_bridge/generic_subscription.hpp @@ -0,0 +1,101 @@ +// 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 cce4273e91f729a98f799ab9aa973626fd8a8a73 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 28 Oct 2021 11:06:18 -0700 Subject: [PATCH 02/28] Update CI to target Foxy Signed-off-by: Jacob Perron --- .github/workflows/build_and_test.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build_and_test.yaml b/.github/workflows/build_and_test.yaml index 56318fe..ce2bbad 100644 --- a/.github/workflows/build_and_test.yaml +++ b/.github/workflows/build_and_test.yaml @@ -10,8 +10,8 @@ jobs: - uses: ros-tooling/setup-ros@v0.2 with: install-connext: true - required-ros-distributions: galactic + required-ros-distributions: foxy - uses: ros-tooling/action-ros-ci@v0.2 with: package-name: domain_bridge - target-ros2-distro: galactic + target-ros2-distro: foxy From 9f3b66adcaf892c334330152bf3c174bc3c6d0a1 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 28 Oct 2021 19:23:40 -0400 Subject: [PATCH 03/28] Use explicit `rclcpp::Duration(int64_t ns)` ctor This constructor has since been deprecated in favor of `rclcpp::Duration::from_nanoseconds`, but that also means it won't be backported to Foxy. Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/domain_bridge.cpp | 9 ++++----- test/domain_bridge/test_qos_matching.cpp | 6 +++--- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 991e2fa..af53973 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -344,10 +344,9 @@ class DomainBridgeImpl if (qos_options.deadline()) { const auto deadline_ns = qos_options.deadline().value(); if (deadline_ns < 0) { - qos.deadline( - rclcpp::Duration::from_nanoseconds(std::numeric_limits::max())); + qos.deadline(rclcpp::Duration(std::numeric_limits::max())); } else { - qos.deadline(rclcpp::Duration::from_nanoseconds(deadline_ns)); + qos.deadline(rclcpp::Duration(deadline_ns)); } } else { qos.deadline(qos_match.qos.deadline()); @@ -356,9 +355,9 @@ class DomainBridgeImpl const auto lifespan_ns = qos_options.lifespan().value(); if (lifespan_ns < 0) { qos.lifespan( - rclcpp::Duration::from_nanoseconds(std::numeric_limits::max())); + rclcpp::Duration(std::numeric_limits::max())); } else { - qos.lifespan(rclcpp::Duration::from_nanoseconds(lifespan_ns)); + qos.lifespan(rclcpp::Duration(lifespan_ns)); } } else { qos.lifespan(qos_match.qos.lifespan()); diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index 02de7fd..bfa10f2 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -95,7 +95,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) EXPECT_EQ(bridged_qos.durability(), qos.durability()); EXPECT_EQ(bridged_qos.liveliness(), qos.liveliness()); // Deadline and lifespan default to max - auto max_duration = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + auto max_duration = rclcpp::Duration(std::numeric_limits::max()); EXPECT_EQ(bridged_qos.deadline(), max_duration); EXPECT_EQ(bridged_qos.lifespan(), max_duration); } @@ -139,7 +139,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) EXPECT_EQ(bridged_qos.durability(), qos.durability()); EXPECT_EQ(bridged_qos.liveliness(), qos.liveliness()); // Deadline and lifespan default to max - auto max_duration = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + auto max_duration = rclcpp::Duration(std::numeric_limits::max()); EXPECT_EQ(bridged_qos.deadline(), max_duration); EXPECT_EQ(bridged_qos.lifespan(), max_duration); } @@ -181,7 +181,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers EXPECT_EQ(bridged_qos.durability(), rclcpp::DurabilityPolicy::Volatile); EXPECT_EQ(bridged_qos.liveliness(), qos.liveliness()); // Deadline and lifespan default to max - auto max_duration = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + auto max_duration = rclcpp::Duration(std::numeric_limits::max()); EXPECT_EQ(bridged_qos.deadline(), max_duration); EXPECT_EQ(bridged_qos.lifespan(), max_duration); } From 925a5e1798f21c53103e87d4f96f09adc1c9f940 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 28 Oct 2021 20:24:12 -0400 Subject: [PATCH 04/28] Obtain domain ID from `rcl_node_options_t` This is because the `rclcpp::Context::get_domain_id` functionality is missing in Foxy. Signed-off-by: Abrar Rahman Protyasha --- CMakeLists.txt | 3 +++ package.xml | 2 ++ src/domain_bridge/wait_for_graph_events.hpp | 7 +++++-- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2588b06..e8a4ca8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) # Leverage rosbag2's generic type support utilities @@ -52,6 +53,7 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif() ament_target_dependencies(${PROJECT_NAME}_lib + rcl rclcpp rcutils rosbag2_cpp @@ -124,6 +126,7 @@ install(DIRECTORY examples launch ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( + rcl rclcpp rosbag2_cpp rcutils diff --git a/package.xml b/package.xml index 853fdca..f501a30 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,8 @@ ament_cmake rosidl_default_generators + rcl + rclcpp rcutils rosbag2_cpp diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index 75c1485..9682f7c 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -28,6 +28,7 @@ #include #include +#include "rcl/node_options.h" #include "rclcpp/client.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" @@ -236,8 +237,9 @@ class WaitForGraphEvents // and print a warning if (reliable_count > 0u && reliable_count != num_endpoints) { result_qos.qos.best_effort(); + const size_t domain_id = (node.get_node_options().get_rcl_node_options())->domain_id; std::string warning = "Some, but not all, publishers on topic '" + topic + - "' on domain ID " + std::to_string(node.get_node_options().context()->get_domain_id()) + + "' on domain ID " + std::to_string(domain_id) + " offer 'reliable' reliability. Falling back to 'best effort' reliability in order " "to connect to all publishers."; result_qos.warnings.push_back(warning); @@ -247,8 +249,9 @@ class WaitForGraphEvents // and print a warning if (transient_local_count > 0u && transient_local_count != num_endpoints) { result_qos.qos.durability_volatile(); + const size_t domain_id = (node.get_node_options().get_rcl_node_options())->domain_id; std::string warning = "Some, but not all, publishers on topic '" + topic + - "' on domain ID " + std::to_string(node.get_node_options().context()->get_domain_id()) + + "' on domain ID " + std::to_string(domain_id) + " offer 'transient local' durability. Falling back to 'volatile' durability in order " "to connect to all publishers."; result_qos.warnings.push_back(warning); From 699ab3a76559f75a6d8ee30bd40d6951fe32aa2a Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Fri, 29 Oct 2021 01:58:32 -0400 Subject: [PATCH 05/28] Use `rmw` types for QoS options class This commit migrates away from the `rclcpp::*Policy` getters and setters used throughout this package, instead just using the equivalent types from `rmw/types.h`. Signed-off-by: Abrar Rahman Protyasha --- CMakeLists.txt | 4 +++- include/domain_bridge/qos_options.hpp | 22 +++++++++---------- package.xml | 1 + src/domain_bridge/domain_bridge.cpp | 9 ++++---- .../parse_domain_bridge_yaml_config.cpp | 14 ++++++------ src/domain_bridge/qos_options.cpp | 14 +++++------- src/domain_bridge/wait_for_graph_events.hpp | 18 ++++++++++----- 7 files changed, 45 insertions(+), 37 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8a4ca8..081777c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,6 +58,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib rcutils rosbag2_cpp rosidl_typesupport_cpp + rmw yaml_cpp_vendor zstd ) @@ -128,8 +129,9 @@ ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( rcl rclcpp - rosbag2_cpp rcutils + rosbag2_cpp + rmw yaml_cpp_vendor zstd_vendor zstd diff --git a/include/domain_bridge/qos_options.hpp b/include/domain_bridge/qos_options.hpp index 8040e6e..044c878 100644 --- a/include/domain_bridge/qos_options.hpp +++ b/include/domain_bridge/qos_options.hpp @@ -17,7 +17,7 @@ #include -#include "rclcpp/qos.hpp" +#include "rmw/types.h" #include "domain_bridge/visibility_control.hpp" @@ -34,7 +34,7 @@ class QosOptions * * - reliability = nullopt_t (detect automatically) * - durability = nullopt_t (detect automatically) - * - history = rclcpp::HistoryPolicy::KeepLast + * - history = RMW_QOS_POLICY_HISTORY_KEEP_LAST * - depth = 10 * - deadline = 0 (RMW default) * - lifespan = 0 (RMW default) @@ -44,33 +44,33 @@ class QosOptions /// Get reliability. DOMAIN_BRIDGE_PUBLIC - std::optional + std::optional reliability() const; /// Set reliability. DOMAIN_BRIDGE_PUBLIC QosOptions & - reliability(const rclcpp::ReliabilityPolicy & reliability); + reliability(rmw_qos_reliability_policy_t reliability); /// Get durability. DOMAIN_BRIDGE_PUBLIC - std::optional + std::optional durability() const; /// Set durability. DOMAIN_BRIDGE_PUBLIC QosOptions & - durability(const rclcpp::DurabilityPolicy & durability); + durability(rmw_qos_durability_policy_t durability); /// Get history. DOMAIN_BRIDGE_PUBLIC - rclcpp::HistoryPolicy + rmw_qos_history_policy_t history() const; /// Set history. DOMAIN_BRIDGE_PUBLIC QosOptions & - history(const rclcpp::HistoryPolicy & history); + history(rmw_qos_history_policy_t history); /// Get history depth. DOMAIN_BRIDGE_PUBLIC @@ -129,9 +129,9 @@ class QosOptions lifespan_auto(); private: - std::optional reliability_; - std::optional durability_; - rclcpp::HistoryPolicy history_{rclcpp::HistoryPolicy::KeepLast}; + std::optional reliability_; + std::optional durability_; + rmw_qos_history_policy_t history_{RMW_QOS_POLICY_HISTORY_KEEP_LAST}; std::size_t depth_{10}; std::optional deadline_{0}; std::optional lifespan_{0}; diff --git a/package.xml b/package.xml index f501a30..e956537 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,7 @@ rosidl_default_generators rcl + rmw rclcpp rcutils diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index af53973..beed27d 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -334,12 +334,12 @@ class DomainBridgeImpl if (qos_options.reliability()) { qos.reliability(qos_options.reliability().value()); } else { - qos.reliability(qos_match.qos.reliability()); + qos.reliability(qos_match.qos.get_rmw_qos_profile().reliability); } if (qos_options.durability()) { qos.durability(qos_options.durability().value()); } else { - qos.durability(qos_match.qos.durability()); + qos.durability(qos_match.qos.get_rmw_qos_profile().durability); } if (qos_options.deadline()) { const auto deadline_ns = qos_options.deadline().value(); @@ -363,8 +363,9 @@ class DomainBridgeImpl qos.lifespan(qos_match.qos.lifespan()); } - qos.liveliness(qos_match.qos.liveliness()); - qos.liveliness_lease_duration(qos_match.qos.liveliness_lease_duration()); + qos.liveliness(qos_match.qos.get_rmw_qos_profile().liveliness); + qos.liveliness_lease_duration( + qos_match.qos.get_rmw_qos_profile().liveliness_lease_duration); // Print any match warnings for (const auto & warning : qos_match.warnings) { diff --git a/src/domain_bridge/parse_domain_bridge_yaml_config.cpp b/src/domain_bridge/parse_domain_bridge_yaml_config.cpp index bc80803..b96e0b5 100644 --- a/src/domain_bridge/parse_domain_bridge_yaml_config.cpp +++ b/src/domain_bridge/parse_domain_bridge_yaml_config.cpp @@ -24,7 +24,7 @@ #include "domain_bridge/topic_bridge_options.hpp" #include "domain_bridge/qos_options.hpp" -#include "rclcpp/qos.hpp" +#include "rmw/types.h" #include "domain_bridge/parse_domain_bridge_yaml_config.hpp" @@ -49,9 +49,9 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi try { auto reliability_str = qos_node["reliability"].as(); if ("reliable" == reliability_str) { - options.reliability(rclcpp::ReliabilityPolicy::Reliable); + options.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); } else if ("best_effort" == reliability_str) { - options.reliability(rclcpp::ReliabilityPolicy::BestEffort); + options.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); } else { throw YamlParsingError( file_path, "unsupported reliability policy value '" + reliability_str + "'"); @@ -65,9 +65,9 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi try { auto durability_str = qos_node["durability"].as(); if ("volatile" == durability_str) { - options.durability(rclcpp::DurabilityPolicy::Volatile); + options.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); } else if ("transient_local" == durability_str) { - options.durability(rclcpp::DurabilityPolicy::TransientLocal); + options.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); } else { throw YamlParsingError( file_path, "unsupported durability policy value '" + durability_str + "'"); @@ -81,9 +81,9 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi try { auto history_str = qos_node["history"].as(); if ("keep_last" == history_str) { - options.history(rclcpp::HistoryPolicy::KeepLast); + options.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST); } else if ("keep_all" == history_str) { - options.history(rclcpp::HistoryPolicy::KeepAll); + options.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL); } else { throw YamlParsingError(file_path, "unsupported history policy value '" + history_str + "'"); } diff --git a/src/domain_bridge/qos_options.cpp b/src/domain_bridge/qos_options.cpp index ac8b1a9..cc8a3f4 100644 --- a/src/domain_bridge/qos_options.cpp +++ b/src/domain_bridge/qos_options.cpp @@ -16,47 +16,45 @@ #include -#include "rclcpp/qos.hpp" - #include "domain_bridge/qos_options.hpp" namespace domain_bridge { -std::optional +std::optional QosOptions::reliability() const { return reliability_; } QosOptions & -QosOptions::reliability(const rclcpp::ReliabilityPolicy & reliability) +QosOptions::reliability(rmw_qos_reliability_policy_t reliability) { reliability_.emplace(reliability); return *this; } -std::optional +std::optional QosOptions::durability() const { return durability_; } QosOptions & -QosOptions::durability(const rclcpp::DurabilityPolicy & durability) +QosOptions::durability(rmw_qos_durability_policy_t durability) { durability_.emplace(durability); return *this; } -rclcpp::HistoryPolicy +rmw_qos_history_policy_t QosOptions::history() const { return history_; } QosOptions & -QosOptions::history(const rclcpp::HistoryPolicy & history) +QosOptions::history(rmw_qos_history_policy_t history) { history_ = history; return *this; diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index 9682f7c..9f1f2b0 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -32,6 +32,8 @@ #include "rclcpp/client.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" +#include "rmw/qos_profiles.h" +#include "rmw/types.h" namespace domain_bridge { @@ -205,10 +207,14 @@ class WaitForGraphEvents // Initialize QoS QosMatchInfo result_qos; // Default reliability and durability to value of first endpoint - result_qos.qos.reliability(endpoint_info_vec[0].qos_profile().reliability()); - result_qos.qos.durability(endpoint_info_vec[0].qos_profile().durability()); + rmw_qos_reliability_policy_t reliability_policy = + endpoint_info_vec[0].qos_profile().get_rmw_qos_profile().reliability; + rmw_qos_durability_policy_t durability_policy = + endpoint_info_vec[0].qos_profile().get_rmw_qos_profile().durability; + result_qos.qos.reliability(reliability_policy); + result_qos.qos.durability(durability_policy); // Always use automatic liveliness - result_qos.qos.liveliness(rclcpp::LivelinessPolicy::Automatic); + result_qos.qos.liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); // Reliability and durability policies can cause trouble with enpoint matching // Count number of "reliable" publishers and number of "transient local" publishers @@ -218,11 +224,11 @@ class WaitForGraphEvents rclcpp::Duration max_deadline(0, 0u); rclcpp::Duration max_lifespan(0, 0u); for (const auto & info : endpoint_info_vec) { - const auto & profile = info.qos_profile(); - if (profile.reliability() == rclcpp::ReliabilityPolicy::Reliable) { + const auto & profile = info.qos_profile().get_rmw_qos_profile(); + if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { reliable_count++; } - if (profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { transient_local_count++; } if (profile.deadline() > max_deadline) { From 1562d57950f6bf67a15bc1c42734008783d89a19 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Fri, 29 Oct 2021 02:00:24 -0400 Subject: [PATCH 06/28] Define a `rmw_time_t` to `rclcpp::Duration` func This commit introduces a utility header that defines a conversion function from `rmw_time_t` to `rclcpp::Duration`. This conversion function is necessary because `rmw_time_t` holds time values (both sec/nsec) as `uint64_t` fields, but `rclcpp::Duration` has a constructor that accepts sec as a `int32_t` value, raising the concern of a narrowing conversion. Similar in spirit to a function with a similar signature introduced in https://github.com/ros2/rclcpp/pull/1467. Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/domain_bridge.cpp | 9 ++++- src/domain_bridge/utils.hpp | 42 +++++++++++++++++++++ src/domain_bridge/wait_for_graph_events.hpp | 13 +++++-- 3 files changed, 58 insertions(+), 6 deletions(-) create mode 100644 src/domain_bridge/utils.hpp diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index beed27d..d066f60 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -45,6 +45,7 @@ #include "generic_publisher.hpp" #include "generic_subscription.hpp" #include "wait_for_graph_events.hpp" +#include "utils.hpp" namespace domain_bridge { @@ -349,7 +350,9 @@ class DomainBridgeImpl qos.deadline(rclcpp::Duration(deadline_ns)); } } else { - qos.deadline(qos_match.qos.deadline()); + rmw_time_t rmw_deadline{qos_match.qos.get_rmw_qos_profile().deadline}; + rclcpp::Duration deadline{utils::from_rmw_time(rmw_deadline)}; + qos.deadline(deadline); } if (qos_options.lifespan()) { const auto lifespan_ns = qos_options.lifespan().value(); @@ -360,7 +363,9 @@ class DomainBridgeImpl qos.lifespan(rclcpp::Duration(lifespan_ns)); } } else { - qos.lifespan(qos_match.qos.lifespan()); + rmw_time_t rmw_lifespan{qos_match.qos.get_rmw_qos_profile().lifespan}; + rclcpp::Duration lifespan{utils::from_rmw_time(rmw_lifespan)}; + qos.lifespan(lifespan); } qos.liveliness(qos_match.qos.get_rmw_qos_profile().liveliness); diff --git a/src/domain_bridge/utils.hpp b/src/domain_bridge/utils.hpp new file mode 100644 index 0000000..225e8cf --- /dev/null +++ b/src/domain_bridge/utils.hpp @@ -0,0 +1,42 @@ +// Copyright 2021, 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 DOMAIN_BRIDGE__UTILS_HPP_ +#define DOMAIN_BRIDGE__UTILS_HPP_ + +#include "rcl/time.h" +#include "rclcpp/duration.hpp" +#include "rmw/types.h" + +namespace domain_bridge +{ + namespace utils + { + rclcpp::Duration from_rmw_time(rmw_time_t duration) + { + constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); + constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); + if (duration.sec > limit_sec || duration.nsec > limit_ns) { + return rclcpp::Duration{limit_ns}; + } + uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; + if (total_ns > limit_ns) { + return rclcpp::Duration{limit_ns}; + } + return rclcpp::Duration{static_cast(total_ns)}; + } + } // namespace utils +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__UTILS_HPP_ diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index 9f1f2b0..f67c3a5 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -30,11 +30,14 @@ #include "rcl/node_options.h" #include "rclcpp/client.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rmw/qos_profiles.h" #include "rmw/types.h" +#include "utils.hpp" + namespace domain_bridge { @@ -231,11 +234,13 @@ class WaitForGraphEvents if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { transient_local_count++; } - if (profile.deadline() > max_deadline) { - max_deadline = profile.deadline(); + rclcpp::Duration deadline{utils::from_rmw_time(profile.deadline)}; + if (deadline > max_deadline) { + max_deadline = deadline; } - if (profile.lifespan() > max_lifespan) { - max_lifespan = profile.lifespan(); + rclcpp::Duration lifespan{utils::from_rmw_time(profile.lifespan)}; + if (lifespan > max_lifespan) { + max_lifespan = lifespan; } } From cfb36cf38a2f9a5f26708323736430c81cc0cbdd Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Fri, 29 Oct 2021 02:14:00 -0400 Subject: [PATCH 07/28] Remove unused function Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/domain_bridge.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index d066f60..beba0cb 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -50,18 +50,6 @@ namespace domain_bridge { -[[noreturn]] static void unreachable() -{ -#if defined(__has_builtin) -#if __has_builtin(__builtin_unreachable) - __builtin_unreachable(); -#endif -#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) - __builtin_unreachable(); -#endif - throw std::logic_error("This code should be unreachable."); -} - /// Implementation of \ref DomainBridge. class DomainBridgeImpl { From e614643ec8a3ff3ec4858504c21e1b4e080f64be Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Fri, 29 Oct 2021 14:50:36 -0400 Subject: [PATCH 08/28] Update distribution references in documentation Correct "Galactic" references to read "Foxy" where appropriate. Signed-off-by: Abrar Rahman Protyasha --- README.md | 2 +- src/domain_bridge/generic_subscription.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 20c155e..2949673 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ See the [design document](doc/design.md) for more details about how the bridge w ## Prerequisites -- [ROS 2](https://index.ros.org/doc/ros2/Installation) (Galactic or newer) +- [ROS 2](https://index.ros.org/doc/ros2/Installation) (Foxy or newer) ## Installation diff --git a/src/domain_bridge/generic_subscription.hpp b/src/domain_bridge/generic_subscription.hpp index a293777..7100cc6 100644 --- a/src/domain_bridge/generic_subscription.hpp +++ b/src/domain_bridge/generic_subscription.hpp @@ -71,8 +71,8 @@ class GenericSubscription : public rclcpp::SubscriptionBase // 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. + // make this change compatible with all of Foxy, Galactic, and later, we + // leave off the 'override' flag. void handle_serialized_message( const std::shared_ptr & serialized_message, From 3272a4800db648d4351d287ce5ec85acf5e72701 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 00:52:30 -0400 Subject: [PATCH 09/28] Set domain ID through the `rcl` layer Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/domain_bridge.cpp | 22 +----------------- src/domain_bridge/utils.hpp | 36 +++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index beba0cb..1d1d567 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -86,24 +86,6 @@ class DomainBridgeImpl ~DomainBridgeImpl() = default; - rclcpp::Context::SharedPtr create_context_with_domain_id(std::size_t domain_id) - { - auto context = std::make_shared(); - rclcpp::InitOptions options; - options.auto_initialize_logging(false).set_domain_id(domain_id); - context->init(0, nullptr, options); - return context; - } - - rclcpp::NodeOptions create_node_options(rclcpp::Context::SharedPtr context) - { - rclcpp::NodeOptions options; - return options.context(context) - .use_global_arguments(false) - .start_parameter_services(false) - .start_parameter_event_publisher(false); - } - rclcpp::Node::SharedPtr get_node_for_domain(std::size_t domain_id) { auto domain_id_node_pair = node_map_.find(domain_id); @@ -113,11 +95,9 @@ class DomainBridgeImpl if (options_.on_new_domain_callback_) { options_.on_new_domain_callback_(domain_id); } - auto context = create_context_with_domain_id(domain_id); - auto node_options = create_node_options(context); std::ostringstream oss; oss << options_.name() << "_" << std::to_string(domain_id); - auto node = std::make_shared(oss.str(), node_options); + auto node = utils::create_node_with_name_and_domain_id(oss.str(), domain_id); node_map_[domain_id] = node; return node; } diff --git a/src/domain_bridge/utils.hpp b/src/domain_bridge/utils.hpp index 225e8cf..330469d 100644 --- a/src/domain_bridge/utils.hpp +++ b/src/domain_bridge/utils.hpp @@ -15,8 +15,18 @@ #ifndef DOMAIN_BRIDGE__UTILS_HPP_ #define DOMAIN_BRIDGE__UTILS_HPP_ +#include +#include +#include + +#include "rcl/node.h" +#include "rcl/node_options.h" #include "rcl/time.h" +#include "rclcpp/context.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/init_options.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" #include "rmw/types.h" namespace domain_bridge @@ -36,6 +46,32 @@ namespace domain_bridge } return rclcpp::Duration{static_cast(total_ns)}; } + + rclcpp::Node::SharedPtr + create_node_with_name_and_domain_id( + const std::string & name, + std::size_t domain_id) + { + auto context = std::make_shared(); + rclcpp::InitOptions init_options; + init_options.auto_initialize_logging(false); + context->init(0, nullptr, init_options); + + rclcpp::NodeOptions node_options; + node_options.context(context) + .use_global_arguments(false) + .start_parameter_services(false) + .start_parameter_event_publisher(false); + + auto node = std::make_shared(name, node_options); + auto node_base_interface = node->get_node_base_interface(); + rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); + // Hacky work-around because setting domain ID is not a feature in the rclcpp layer + const_cast( + rcl_node_get_options(rcl_node_handle))->domain_id = domain_id; + + return node; + } } // namespace utils } // namespace domain_bridge From baa184d835685b231c46bcd1730028f563ddecd4 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 01:54:12 -0400 Subject: [PATCH 10/28] Explicitly find `rmw` package in CMake Signed-off-by: Abrar Rahman Protyasha --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 081777c..d0d9e3c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(rcutils REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(rmw REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(zstd_vendor REQUIRED) find_package(zstd REQUIRED) From 549654aaf6047bb28a474d7ffccdbebf0a9886d5 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 01:54:48 -0400 Subject: [PATCH 11/28] Move utility functions to library headers This will allow the test code to also access said utility functions, with the primary purpose being to alleviate the lack of a `set_domain_id` functionality in `rclcpp` in Foxy. Signed-off-by: Abrar Rahman Protyasha --- CMakeLists.txt | 1 + include/domain_bridge/utils.hpp | 43 ++++++++++++ src/domain_bridge/domain_bridge.cpp | 2 +- src/domain_bridge/utils.cpp | 75 ++++++++++++++++++++ src/domain_bridge/utils.hpp | 78 --------------------- src/domain_bridge/wait_for_graph_events.hpp | 2 +- 6 files changed, 121 insertions(+), 80 deletions(-) create mode 100644 include/domain_bridge/utils.hpp create mode 100644 src/domain_bridge/utils.cpp delete mode 100644 src/domain_bridge/utils.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d0d9e3c..43b2da5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,7 @@ add_library(${PROJECT_NAME}_lib SHARED src/${PROJECT_NAME}/qos_options.cpp src/${PROJECT_NAME}/service_bridge_options.cpp src/${PROJECT_NAME}/topic_bridge_options.cpp + src/${PROJECT_NAME}/utils.cpp ) target_include_directories(${PROJECT_NAME}_lib PUBLIC diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp new file mode 100644 index 0000000..36d534b --- /dev/null +++ b/include/domain_bridge/utils.hpp @@ -0,0 +1,43 @@ +// Copyright 2021, 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 DOMAIN_BRIDGE__UTILS_HPP_ +#define DOMAIN_BRIDGE__UTILS_HPP_ + +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "rmw/types.h" + +#include "domain_bridge/visibility_control.hpp" + +namespace domain_bridge +{ + +namespace utils +{ + + DOMAIN_BRIDGE_PUBLIC + rclcpp::Duration from_rmw_time(rmw_time_t duration); + + DOMAIN_BRIDGE_PUBLIC + rclcpp::Node::SharedPtr + create_node_with_name_and_domain_id(const std::string & name, std::size_t domain_id); + +} // namespace utils + +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__UTILS_HPP_ diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 1d1d567..8e64f5d 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -41,11 +41,11 @@ #include "domain_bridge/topic_bridge.hpp" #include "domain_bridge/topic_bridge_options.hpp" #include "domain_bridge/msg/compressed_msg.hpp" +#include "domain_bridge/utils.hpp" #include "generic_publisher.hpp" #include "generic_subscription.hpp" #include "wait_for_graph_events.hpp" -#include "utils.hpp" namespace domain_bridge { diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp new file mode 100644 index 0000000..8c79a2e --- /dev/null +++ b/src/domain_bridge/utils.cpp @@ -0,0 +1,75 @@ +// Copyright 2021, 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 +#include + +#include "rcl/node.h" +#include "rcl/node_options.h" +#include "rcl/time.h" +#include "rclcpp/context.hpp" +#include "rclcpp/init_options.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" + +#include "domain_bridge/utils.hpp" + +namespace domain_bridge +{ + +namespace utils +{ + + rclcpp::Duration from_rmw_time(rmw_time_t duration) + { + constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); + constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); + if (duration.sec > limit_sec || duration.nsec > limit_ns) { + return rclcpp::Duration{limit_ns}; + } + uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; + if (total_ns > limit_ns) { + return rclcpp::Duration{limit_ns}; + } + return rclcpp::Duration{static_cast(total_ns)}; + } + + rclcpp::Node::SharedPtr + create_node_with_name_and_domain_id( + const std::string & name, + std::size_t domain_id) + { + auto context = std::make_shared(); + rclcpp::InitOptions init_options; + init_options.auto_initialize_logging(false); + context->init(0, nullptr, init_options); + + rclcpp::NodeOptions node_options; + node_options.context(context) + .use_global_arguments(false) + .start_parameter_services(false) + .start_parameter_event_publisher(false); + + auto node = std::make_shared(name, node_options); + auto node_base_interface = node->get_node_base_interface(); + rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); + // Hacky work-around because setting domain ID is not a feature in the rclcpp layer + const_cast( + rcl_node_get_options(rcl_node_handle))->domain_id = domain_id; + + return node; + } + +} // namespace utils + +} // namespace domain_bridge diff --git a/src/domain_bridge/utils.hpp b/src/domain_bridge/utils.hpp deleted file mode 100644 index 330469d..0000000 --- a/src/domain_bridge/utils.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// Copyright 2021, 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 DOMAIN_BRIDGE__UTILS_HPP_ -#define DOMAIN_BRIDGE__UTILS_HPP_ - -#include -#include -#include - -#include "rcl/node.h" -#include "rcl/node_options.h" -#include "rcl/time.h" -#include "rclcpp/context.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/init_options.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/node_interfaces/node_base.hpp" -#include "rmw/types.h" - -namespace domain_bridge -{ - namespace utils - { - rclcpp::Duration from_rmw_time(rmw_time_t duration) - { - constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); - constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); - if (duration.sec > limit_sec || duration.nsec > limit_ns) { - return rclcpp::Duration{limit_ns}; - } - uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; - if (total_ns > limit_ns) { - return rclcpp::Duration{limit_ns}; - } - return rclcpp::Duration{static_cast(total_ns)}; - } - - rclcpp::Node::SharedPtr - create_node_with_name_and_domain_id( - const std::string & name, - std::size_t domain_id) - { - auto context = std::make_shared(); - rclcpp::InitOptions init_options; - init_options.auto_initialize_logging(false); - context->init(0, nullptr, init_options); - - rclcpp::NodeOptions node_options; - node_options.context(context) - .use_global_arguments(false) - .start_parameter_services(false) - .start_parameter_event_publisher(false); - - auto node = std::make_shared(name, node_options); - auto node_base_interface = node->get_node_base_interface(); - rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); - // Hacky work-around because setting domain ID is not a feature in the rclcpp layer - const_cast( - rcl_node_get_options(rcl_node_handle))->domain_id = domain_id; - - return node; - } - } // namespace utils -} // namespace domain_bridge - -#endif // DOMAIN_BRIDGE__UTILS_HPP_ diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index f67c3a5..c4043b8 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -36,7 +36,7 @@ #include "rmw/qos_profiles.h" #include "rmw/types.h" -#include "utils.hpp" +#include "domain_bridge/utils.hpp" namespace domain_bridge { From eb58092084f524447d3c4424bf692ad0b75f1866 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 02:24:54 -0400 Subject: [PATCH 12/28] Use utility func to set node domain ID in tests Signed-off-by: Abrar Rahman Protyasha --- .../test_domain_bridge_services.cpp | 25 ++---- test/domain_bridge/test_qos_matching.cpp | 79 +++++++------------ 2 files changed, 33 insertions(+), 71 deletions(-) diff --git a/test/domain_bridge/test_domain_bridge_services.cpp b/test/domain_bridge/test_domain_bridge_services.cpp index 84be850..9a7571a 100644 --- a/test/domain_bridge/test_domain_bridge_services.cpp +++ b/test/domain_bridge/test_domain_bridge_services.cpp @@ -26,6 +26,7 @@ #include "test_msgs/srv/empty.hpp" #include "domain_bridge/domain_bridge.hpp" +#include "domain_bridge/utils.hpp" static constexpr std::size_t kDomain1{1u}; @@ -38,28 +39,12 @@ class TestDomainBridgeServices : public ::testing::Test protected: void SetUp() override { - // Initialize contexts in different domains - rclcpp::InitOptions context_options; - - context_1_ = std::make_shared(); - context_options.auto_initialize_logging(true).set_domain_id(kDomain1); - context_1_->init(0, nullptr, context_options); - - context_2_ = std::make_shared(); - context_options.auto_initialize_logging(false).set_domain_id(kDomain2); - context_2_->init(0, nullptr, context_options); - // Initialize one node in each domain - rclcpp::NodeOptions node_options; - - node_options.context(context_1_); - node_1_ = std::make_shared("node_1", node_options); - - node_options.context(context_2_); - node_2_ = std::make_shared("node_2", node_options); + auto node_1_ = domain_bridge::utils::create_node_with_name_and_domain_id( + "node_1", kDomain1); + auto node_2_ = domain_bridge::utils::create_node_with_name_and_domain_id( + "node_2", kDomain2); } - std::shared_ptr context_1_; - std::shared_ptr context_2_; std::shared_ptr node_1_; std::shared_ptr node_2_; }; diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index bfa10f2..e938f8b 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -24,51 +24,27 @@ #include "test_msgs/msg/basic_types.hpp" #include "domain_bridge/domain_bridge.hpp" +#include "domain_bridge/utils.hpp" #include "wait_for_publisher.hpp" class TestDomainBridgeQosMatching : public ::testing::Test { protected: - static void SetUpTestCase() - { - // Initialize contexts in different domains - context_1_ = std::make_shared(); - rclcpp::InitOptions context_options_1; - context_options_1.auto_initialize_logging(false).set_domain_id(domain_1_); - context_1_->init(0, nullptr, context_options_1); - - context_2_ = std::make_shared(); - rclcpp::InitOptions context_options_2; - context_options_2.auto_initialize_logging(false).set_domain_id(domain_2_); - context_2_->init(0, nullptr, context_options_2); - - node_options_1_.context(context_1_); - node_options_2_.context(context_2_); - } - - static const std::size_t domain_1_{1u}; - static const std::size_t domain_2_{2u}; - static std::shared_ptr context_1_; - static std::shared_ptr context_2_; - static rclcpp::NodeOptions node_options_1_; - static rclcpp::NodeOptions node_options_2_; + static constexpr std::size_t domain_1_{1u}; + static constexpr std::size_t domain_2_{2u}; }; -const std::size_t TestDomainBridgeQosMatching::domain_1_; -const std::size_t TestDomainBridgeQosMatching::domain_2_; -std::shared_ptr TestDomainBridgeQosMatching::context_1_; -std::shared_ptr TestDomainBridgeQosMatching::context_2_; -rclcpp::NodeOptions TestDomainBridgeQosMatching::node_options_1_; -rclcpp::NodeOptions TestDomainBridgeQosMatching::node_options_2_; +constexpr std::size_t TestDomainBridgeQosMatching::domain_1_; +constexpr std::size_t TestDomainBridgeQosMatching::domain_2_; TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) { const std::string topic_name("test_topic_exists_before_bridge"); // Create a publisher on domain 1 - auto node_1 = std::make_shared( - "test_topic_exists_before_bridge_node_1", node_options_1_); + auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_exists_before_bridge_node_1", domain_1_); rclcpp::QoS qos(1); qos.best_effort() .transient_local() @@ -82,8 +58,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = std::make_shared( - "test_topic_exists_before_bridge_node_2", node_options_2_); + auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_exists_before_bridge_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the QoS of the bridged publisher matches @@ -104,8 +80,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) { const std::string topic_name("test_topic_exists_after_bridge"); - auto node_1 = std::make_shared( - "test_topic_exists_after_bridge_node_1", node_options_1_); + auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_exists_after_bridge_node_1", domain_1_); // Bridge the publisher topic to domain 2 domain_bridge::DomainBridge bridge; @@ -113,8 +89,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) // Wait for bridge publisher to appear on domain 2 // It shouldn't be available yet - auto node_2 = std::make_shared( - "test_topic_exists_after_bridge_node_2", node_options_2_); + auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_exists_after_bridge_node_2", domain_2_); ASSERT_FALSE(wait_for_publisher(node_2, topic_name, std::chrono::milliseconds(300))); // Create a publisher on domain 1 @@ -149,8 +125,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers const std::string topic_name("test_topic_exists_multiple_publishers"); // Create two publishers on domain 1 - auto node_1 = std::make_shared( - "test_topic_exists_multiple_publishers_node_1", node_options_1_); + auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_exists_multiple_publishers_node_1", domain_1_); rclcpp::QoS qos(1); qos.reliable() .durability_volatile() @@ -167,8 +143,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = std::make_shared( - "test_topic_exists_multiple_publishers_node_2", node_options_2_); + auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_exists_multiple_publishers_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the QoS of the bridged publishers matches both publishers @@ -195,8 +171,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_does_not_exist) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // We do not expect a bridge publisher to appear because there is no input publisher - auto node_2 = std::make_shared( - "test_topic_does_not_exist_node_2", node_options_2_); + auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_topic_does_not_exist_node_2", domain_2_); ASSERT_FALSE(wait_for_publisher(node_2, topic_name, std::chrono::seconds(1))); } @@ -205,8 +181,9 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) const std::string topic_name("test_always_automatic_liveliness"); // Create a publisher on domain 1 with liveliness set to "manual by topic" - auto node_1 = std::make_shared( - "test_always_automatic_liveliness_node_1", node_options_1_); + auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_always_automatic_liveliness_node_1", domain_1_); + rclcpp::QoS qos(1); qos.liveliness(rclcpp::LivelinessPolicy::ManualByTopic); auto pub = node_1->create_publisher(topic_name, qos); @@ -216,8 +193,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = std::make_shared( - "test_always_automatic_liveliness_node_2", node_options_2_); + auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_always_automatic_liveliness_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the liveliness policy is "automatic", not "manual by topic" @@ -235,8 +212,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) // Create two publishers on domain 1 // The first deadline will be greater than the second deadline // The second lifespan will be greater than the first lifespan - auto node_1 = std::make_shared( - "test_max_of_duration_policy_node_1", node_options_1_); + auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_max_of_duration_policy_node_1", domain_1_); rclcpp::QoS qos_1(1); qos_1.deadline(rclcpp::Duration(554, 321u)) .lifespan(rclcpp::Duration(123, 456u)); @@ -257,8 +234,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_, bridge_options); // Wait for bridge publisher to appear on domain 2 - auto node_2 = std::make_shared( - "test_max_of_duration_policy_node_2", node_options_2_); + auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + "test_max_of_duration_policy_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert max of the two deadline and lifespan policies are used for the bridge QoS From ffd9bc328b019f61feee998ac321c688783388d4 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 16:49:22 -0400 Subject: [PATCH 13/28] Linter fixes in the utility functions Signed-off-by: Abrar Rahman Protyasha --- include/domain_bridge/utils.hpp | 14 ++++--- src/domain_bridge/utils.cpp | 73 ++++++++++++++++++--------------- 2 files changed, 49 insertions(+), 38 deletions(-) diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp index 36d534b..c8cc2e5 100644 --- a/include/domain_bridge/utils.hpp +++ b/include/domain_bridge/utils.hpp @@ -16,6 +16,7 @@ #define DOMAIN_BRIDGE__UTILS_HPP_ #include +#include #include "rclcpp/duration.hpp" #include "rclcpp/node.hpp" @@ -29,12 +30,15 @@ namespace domain_bridge namespace utils { - DOMAIN_BRIDGE_PUBLIC - rclcpp::Duration from_rmw_time(rmw_time_t duration); +DOMAIN_BRIDGE_PUBLIC +rclcpp::Duration from_rmw_time(rmw_time_t duration); - DOMAIN_BRIDGE_PUBLIC - rclcpp::Node::SharedPtr - create_node_with_name_and_domain_id(const std::string & name, std::size_t domain_id); +DOMAIN_BRIDGE_PUBLIC +rclcpp::Node::SharedPtr +create_node( + const std::string & name, + std::size_t domain_id, + std::shared_ptr context = nullptr); } // namespace utils diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 8c79a2e..17409f2 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -14,12 +14,14 @@ #include #include +#include #include "rcl/node.h" #include "rcl/node_options.h" #include "rcl/time.h" #include "rclcpp/context.hpp" #include "rclcpp/init_options.hpp" +#include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_base.hpp" #include "domain_bridge/utils.hpp" @@ -30,44 +32,49 @@ namespace domain_bridge namespace utils { - rclcpp::Duration from_rmw_time(rmw_time_t duration) - { - constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); - constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); - if (duration.sec > limit_sec || duration.nsec > limit_ns) { - return rclcpp::Duration{limit_ns}; - } - uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; - if (total_ns > limit_ns) { - return rclcpp::Duration{limit_ns}; - } - return rclcpp::Duration{static_cast(total_ns)}; +rclcpp::Duration from_rmw_time(rmw_time_t duration) +{ + constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); + constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); + if (duration.sec > limit_sec || duration.nsec > limit_ns) { + return rclcpp::Duration{limit_ns}; + } + uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; + if (total_ns > limit_ns) { + return rclcpp::Duration{limit_ns}; + } + return rclcpp::Duration{static_cast(total_ns)}; +} + +rclcpp::Node::SharedPtr +create_node( + const std::string & name, + std::size_t domain_id, + std::shared_ptr context) +{ + if (context == nullptr) { + context = std::make_shared(); } + rclcpp::InitOptions init_options; + init_options.auto_initialize_logging(false); + context->init(0, nullptr, init_options); - rclcpp::Node::SharedPtr - create_node_with_name_and_domain_id( - const std::string & name, - std::size_t domain_id) - { - auto context = std::make_shared(); - rclcpp::InitOptions init_options; - init_options.auto_initialize_logging(false); - context->init(0, nullptr, init_options); + rclcpp::NodeOptions node_options; + node_options.context(context) + .use_global_arguments(false) + .start_parameter_services(false) + .start_parameter_event_publisher(false); - rclcpp::NodeOptions node_options; - node_options.context(context) - .use_global_arguments(false) - .start_parameter_services(false) - .start_parameter_event_publisher(false); + auto node = std::make_shared(name, node_options); + auto node_base_interface = node->get_node_base_interface(); + rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); + // Hacky work-around because setting domain ID is not a feature in the rclcpp layer + const_cast( + rcl_node_get_options(rcl_node_handle))->domain_id = domain_id; - auto node = std::make_shared(name, node_options); - auto node_base_interface = node->get_node_base_interface(); - rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); - // Hacky work-around because setting domain ID is not a feature in the rclcpp layer - const_cast( - rcl_node_get_options(rcl_node_handle))->domain_id = domain_id; + return node; +} - return node; } } // namespace utils From b619145d9516c872a23e3cfb58b38d26bf106201 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 16:49:51 -0400 Subject: [PATCH 14/28] Add utility function to get domain ID from node Signed-off-by: Abrar Rahman Protyasha --- include/domain_bridge/utils.hpp | 5 +++++ src/domain_bridge/utils.cpp | 21 +++++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp index c8cc2e5..5a145f9 100644 --- a/include/domain_bridge/utils.hpp +++ b/include/domain_bridge/utils.hpp @@ -40,6 +40,11 @@ create_node( std::size_t domain_id, std::shared_ptr context = nullptr); +DOMAIN_BRIDGE_PUBLIC +std::size_t +get_domain_id_from_node( + rclcpp::Node & node); + } // namespace utils } // namespace domain_bridge diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 17409f2..4254f33 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include "rcl/node.h" @@ -75,7 +76,27 @@ create_node( return node; } +std::size_t +get_domain_id_from_node( + rclcpp::Node & node) +{ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface = + node.get_node_base_interface(); + rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); + + std::size_t domain_id; + rcl_ret_t ret = rcl_node_get_domain_id(rcl_node_handle, &domain_id); + if (ret == RCL_RET_OK) { + return domain_id; + } else if (ret == RCL_RET_NODE_INVALID) { + throw std::runtime_error("[get_domain_id_from_node] Node invalid."); + } else if (ret == RCL_RET_INVALID_ARGUMENT) { + throw std::runtime_error("[get_domain_id_from_node] Invalid argument."); + } else { + throw std::runtime_error("[get_domain_id_from_node] Unspecified error."); } + // return (node.get_node_options().get_rcl_node_options())->domain_id; +} } // namespace utils From 80418accb8d4dd55fd7c790840f24cc45d0c535b Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 16:50:25 -0400 Subject: [PATCH 15/28] Use updated util signatures Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/domain_bridge.cpp | 2 +- src/domain_bridge/wait_for_graph_events.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 8e64f5d..07b74fc 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -97,7 +97,7 @@ class DomainBridgeImpl } std::ostringstream oss; oss << options_.name() << "_" << std::to_string(domain_id); - auto node = utils::create_node_with_name_and_domain_id(oss.str(), domain_id); + auto node = utils::create_node(oss.str(), domain_id); node_map_[domain_id] = node; return node; } diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index c4043b8..bf363b0 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -248,7 +248,7 @@ class WaitForGraphEvents // and print a warning if (reliable_count > 0u && reliable_count != num_endpoints) { result_qos.qos.best_effort(); - const size_t domain_id = (node.get_node_options().get_rcl_node_options())->domain_id; + const size_t domain_id = domain_bridge::utils::get_domain_id_from_node(node); std::string warning = "Some, but not all, publishers on topic '" + topic + "' on domain ID " + std::to_string(domain_id) + " offer 'reliable' reliability. Falling back to 'best effort' reliability in order " @@ -260,7 +260,7 @@ class WaitForGraphEvents // and print a warning if (transient_local_count > 0u && transient_local_count != num_endpoints) { result_qos.qos.durability_volatile(); - const size_t domain_id = (node.get_node_options().get_rcl_node_options())->domain_id; + const size_t domain_id = domain_bridge::utils::get_domain_id_from_node(node); std::string warning = "Some, but not all, publishers on topic '" + topic + "' on domain ID " + std::to_string(domain_id) + " offer 'transient local' durability. Falling back to 'volatile' durability in order " From 6e262b8242309eea5e3032c5169b70cba4fbc782 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 16:50:46 -0400 Subject: [PATCH 16/28] Get tests to compile Signed-off-by: Abrar Rahman Protyasha --- .../test_domain_bridge_end_to_end.cpp | 34 ++---- .../test_domain_bridge_services.cpp | 10 +- .../test_parse_domain_bridge_yaml_config.cpp | 8 +- test/domain_bridge/test_qos_matching.cpp | 106 ++++++++++++------ 4 files changed, 93 insertions(+), 65 deletions(-) diff --git a/test/domain_bridge/test_domain_bridge_end_to_end.cpp b/test/domain_bridge/test_domain_bridge_end_to_end.cpp index 0b68239..e468018 100644 --- a/test/domain_bridge/test_domain_bridge_end_to_end.cpp +++ b/test/domain_bridge/test_domain_bridge_end_to_end.cpp @@ -27,6 +27,7 @@ #include "domain_bridge/compress_messages.hpp" #include "domain_bridge/domain_bridge.hpp" #include "domain_bridge/msg/compressed_msg.hpp" +#include "domain_bridge/utils.hpp" static constexpr std::size_t kDomain1{1u}; @@ -40,28 +41,13 @@ class TestDomainBridgeEndToEnd : public ::testing::Test protected: void SetUp() override { - // Initialize contexts in different domains - rclcpp::InitOptions context_options; - context_1_ = std::make_shared(); - context_options.auto_initialize_logging(true).set_domain_id(kDomain1); - context_1_->init(0, nullptr, context_options); - - context_2_ = std::make_shared(); - context_options.auto_initialize_logging(false).set_domain_id(kDomain2); - context_2_->init(0, nullptr, context_options); - - // Initialize one node in each domain - rclcpp::NodeOptions node_options; - - node_options.context(context_1_); - node_1_ = std::make_shared("node_1", node_options); - - node_options.context(context_2_); - node_2_ = std::make_shared("node_2", node_options); + node_1_ = domain_bridge::utils::create_node( + "node_1", kDomain1, context_1_); + node_2_ = domain_bridge::utils::create_node( + "node_2", kDomain2); } std::shared_ptr context_1_; - std::shared_ptr context_2_; std::shared_ptr node_1_; std::shared_ptr node_2_; // use a qos profile with transient local volatility to make the tests simple. @@ -89,7 +75,7 @@ class ScopedAsyncSpinner public: explicit ScopedAsyncSpinner(std::shared_ptr context) : executor_{get_executor_options_with_context(std::move(context))}, - thread_{[this, stop_token = promise_.get_future()] { + thread_{[this, stop_token = std::shared_future{promise_.get_future()}] { executor_.spin_until_future_complete(stop_token); }} {} @@ -136,7 +122,7 @@ TEST_F(TestDomainBridgeEndToEnd, remap_topic_name) auto sub = node_2_->create_subscription( remap_name, pub_sub_qos_, - [&got_message](const test_msgs::msg::BasicTypes &) {got_message = true;}); + [&got_message](test_msgs::msg::BasicTypes::SharedPtr) {got_message = true;}); // Bridge the publisher topic to domain 2 with a remap option domain_bridge::DomainBridge bridge; @@ -167,7 +153,7 @@ TEST_F(TestDomainBridgeEndToEnd, remap_topic_name_with_substitution) auto sub = node_2_->create_subscription( expected_name, pub_sub_qos_, - [&got_message](const test_msgs::msg::BasicTypes &) {got_message = true;}); + [&got_message](test_msgs::msg::BasicTypes::SharedPtr) {got_message = true;}); // Bridge the publisher topic to domain 2 with a remap option domain_bridge::DomainBridgeOptions domain_bridge_options; @@ -198,7 +184,7 @@ TEST_F(TestDomainBridgeEndToEnd, compress_mode) auto sub = node_2_->create_subscription( topic_name, pub_sub_qos_, - [&got_message](const domain_bridge::msg::CompressedMsg &) {got_message = true;}); + [&got_message](domain_bridge::msg::CompressedMsg::SharedPtr) {got_message = true;}); // Bridge the publisher topic to domain 2 with a remap option domain_bridge::DomainBridgeOptions domain_bridge_options; @@ -228,7 +214,7 @@ TEST_F(TestDomainBridgeEndToEnd, decompress_mode) auto sub = node_2_->create_subscription( topic_name, pub_sub_qos_, - [&got_message](const test_msgs::msg::BasicTypes &) {got_message = true;}); + [&got_message](test_msgs::msg::BasicTypes::SharedPtr) {got_message = true;}); // Bridge the publisher topic to domain 2 with a remap option domain_bridge::DomainBridgeOptions domain_bridge_options; diff --git a/test/domain_bridge/test_domain_bridge_services.cpp b/test/domain_bridge/test_domain_bridge_services.cpp index 9a7571a..8c67a23 100644 --- a/test/domain_bridge/test_domain_bridge_services.cpp +++ b/test/domain_bridge/test_domain_bridge_services.cpp @@ -39,12 +39,14 @@ class TestDomainBridgeServices : public ::testing::Test protected: void SetUp() override { + context_1_ = std::make_shared(); // Initialize one node in each domain - auto node_1_ = domain_bridge::utils::create_node_with_name_and_domain_id( - "node_1", kDomain1); - auto node_2_ = domain_bridge::utils::create_node_with_name_and_domain_id( + node_1_ = domain_bridge::utils::create_node( + "node_1", kDomain1, context_1_); + node_2_ = domain_bridge::utils::create_node( "node_2", kDomain2); } + std::shared_ptr context_1_; std::shared_ptr node_1_; std::shared_ptr node_2_; }; @@ -68,7 +70,7 @@ class ScopedAsyncSpinner public: explicit ScopedAsyncSpinner(std::shared_ptr context) : executor_{get_executor_options_with_context(std::move(context))}, - thread_{[this, stop_token = promise_.get_future()] { + thread_{[this, stop_token = std::shared_future{promise_.get_future()}] { executor_.spin_until_future_complete(stop_token); }} {} diff --git a/test/domain_bridge/test_parse_domain_bridge_yaml_config.cpp b/test/domain_bridge/test_parse_domain_bridge_yaml_config.cpp index ad14196..3ce790d 100644 --- a/test/domain_bridge/test_parse_domain_bridge_yaml_config.cpp +++ b/test/domain_bridge/test_parse_domain_bridge_yaml_config.cpp @@ -19,6 +19,8 @@ #include #include +#include "rmw/types.h" + #include "domain_bridge/domain_bridge_config.hpp" #include "domain_bridge/parse_domain_bridge_yaml_config.hpp" #include "domain_bridge/topic_bridge.hpp" @@ -116,9 +118,9 @@ TEST_F(TestParseDomainBridgeYamlConfig, topic_options) domain_bridge::TopicBridgeOptions(), domain_bridge::TopicBridgeOptions().qos_options( domain_bridge::QosOptions() - .reliability(rclcpp::ReliabilityPolicy::BestEffort) - .durability(rclcpp::DurabilityPolicy::TransientLocal) - .history(rclcpp::HistoryPolicy::KeepAll) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + .history(RMW_QOS_POLICY_HISTORY_KEEP_ALL) .depth(42u) .deadline(123456) .lifespan_auto()) diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index e938f8b..03acc20 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -21,6 +22,7 @@ #include "rclcpp/context.hpp" #include "rclcpp/node.hpp" +#include "rmw/types.h" #include "test_msgs/msg/basic_types.hpp" #include "domain_bridge/domain_bridge.hpp" @@ -38,19 +40,25 @@ class TestDomainBridgeQosMatching : public ::testing::Test constexpr std::size_t TestDomainBridgeQosMatching::domain_1_; constexpr std::size_t TestDomainBridgeQosMatching::domain_2_; +std::ostream& operator<<(std::ostream& os, const rclcpp::TopicEndpointInfo& t) +{ + os << t.node_namespace() << '/' << t.node_name() << ": " << t.topic_type(); + return os; +} + TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) { const std::string topic_name("test_topic_exists_before_bridge"); // Create a publisher on domain 1 - auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_1 = domain_bridge::utils::create_node( "test_topic_exists_before_bridge_node_1", domain_1_); rclcpp::QoS qos(1); qos.best_effort() .transient_local() .deadline(rclcpp::Duration(123, 456u)) .lifespan(rclcpp::Duration(554, 321u)) - .liveliness(rclcpp::LivelinessPolicy::Automatic); + .liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); auto pub = node_1->create_publisher(topic_name, qos); // Bridge the publisher topic to domain 2 @@ -58,29 +66,41 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_2 = domain_bridge::utils::create_node( "test_topic_exists_before_bridge_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the QoS of the bridged publisher matches std::vector endpoint_info_vec = node_2->get_publishers_info_by_topic(topic_name); - ASSERT_EQ(endpoint_info_vec.size(), 1u); + for (auto & t : endpoint_info_vec) { + std::cout << t << '\n'; + } + EXPECT_EQ(domain_bridge::utils::get_domain_id_from_node(*node_1), domain_1_); + EXPECT_EQ(domain_bridge::utils::get_domain_id_from_node(*node_2), domain_2_); + EXPECT_EQ(endpoint_info_vec.size(), 1u); + // ASSERT_EQ(endpoint_info_vec.size(), 1u); const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); - EXPECT_EQ(bridged_qos.reliability(), qos.reliability()); - EXPECT_EQ(bridged_qos.durability(), qos.durability()); - EXPECT_EQ(bridged_qos.liveliness(), qos.liveliness()); + rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); + rmw_qos_profile_t rmw_qos_profile = qos.get_rmw_qos_profile(); + EXPECT_EQ(rmw_bridged_qos_profile.reliability, rmw_qos_profile.reliability); + EXPECT_EQ(rmw_bridged_qos_profile.durability, rmw_qos_profile.durability); + EXPECT_EQ(rmw_bridged_qos_profile.liveliness, rmw_qos_profile.liveliness); // Deadline and lifespan default to max auto max_duration = rclcpp::Duration(std::numeric_limits::max()); - EXPECT_EQ(bridged_qos.deadline(), max_duration); - EXPECT_EQ(bridged_qos.lifespan(), max_duration); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.deadline), + max_duration); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.lifespan), + max_duration); } TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) { const std::string topic_name("test_topic_exists_after_bridge"); - auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_1 = domain_bridge::utils::create_node( "test_topic_exists_after_bridge_node_1", domain_1_); // Bridge the publisher topic to domain 2 @@ -89,7 +109,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) // Wait for bridge publisher to appear on domain 2 // It shouldn't be available yet - auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_2 = domain_bridge::utils::create_node( "test_topic_exists_after_bridge_node_2", domain_2_); ASSERT_FALSE(wait_for_publisher(node_2, topic_name, std::chrono::milliseconds(300))); @@ -99,7 +119,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) .transient_local() .deadline(rclcpp::Duration(123, 456u)) .lifespan(rclcpp::Duration(554, 321u)) - .liveliness(rclcpp::LivelinessPolicy::Automatic); + .liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); auto pub = node_1->create_publisher(topic_name, qos); // Wait for bridge publihser to appear on domain 2 @@ -111,13 +131,19 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); - EXPECT_EQ(bridged_qos.reliability(), qos.reliability()); - EXPECT_EQ(bridged_qos.durability(), qos.durability()); - EXPECT_EQ(bridged_qos.liveliness(), qos.liveliness()); + rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); + rmw_qos_profile_t rmw_qos_profile = qos.get_rmw_qos_profile(); + EXPECT_EQ(rmw_bridged_qos_profile.reliability, rmw_qos_profile.reliability); + EXPECT_EQ(rmw_bridged_qos_profile.durability, rmw_qos_profile.durability); + EXPECT_EQ(rmw_bridged_qos_profile.liveliness, rmw_qos_profile.liveliness); // Deadline and lifespan default to max auto max_duration = rclcpp::Duration(std::numeric_limits::max()); - EXPECT_EQ(bridged_qos.deadline(), max_duration); - EXPECT_EQ(bridged_qos.lifespan(), max_duration); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.deadline), + max_duration); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.lifespan), + max_duration); } TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers) @@ -125,14 +151,14 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers const std::string topic_name("test_topic_exists_multiple_publishers"); // Create two publishers on domain 1 - auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_1 = domain_bridge::utils::create_node( "test_topic_exists_multiple_publishers_node_1", domain_1_); rclcpp::QoS qos(1); qos.reliable() .durability_volatile() .deadline(rclcpp::Duration(123, 456u)) .lifespan(rclcpp::Duration(554, 321u)) - .liveliness(rclcpp::LivelinessPolicy::Automatic); + .liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); auto pub_1 = node_1->create_publisher(topic_name, qos); // Second publisher has different QoS qos.best_effort().transient_local(); @@ -143,7 +169,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_2 = domain_bridge::utils::create_node( "test_topic_exists_multiple_publishers_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); @@ -153,13 +179,19 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); - EXPECT_EQ(bridged_qos.reliability(), rclcpp::ReliabilityPolicy::BestEffort); - EXPECT_EQ(bridged_qos.durability(), rclcpp::DurabilityPolicy::Volatile); - EXPECT_EQ(bridged_qos.liveliness(), qos.liveliness()); + rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); + rmw_qos_profile_t rmw_qos_profile = qos.get_rmw_qos_profile(); + EXPECT_EQ(rmw_bridged_qos_profile.reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + EXPECT_EQ(rmw_bridged_qos_profile.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + EXPECT_EQ(rmw_bridged_qos_profile.liveliness, rmw_qos_profile.liveliness); // Deadline and lifespan default to max auto max_duration = rclcpp::Duration(std::numeric_limits::max()); - EXPECT_EQ(bridged_qos.deadline(), max_duration); - EXPECT_EQ(bridged_qos.lifespan(), max_duration); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.deadline), + max_duration); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.lifespan), + max_duration); } TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_does_not_exist) @@ -171,7 +203,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_does_not_exist) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // We do not expect a bridge publisher to appear because there is no input publisher - auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_2 = domain_bridge::utils::create_node( "test_topic_does_not_exist_node_2", domain_2_); ASSERT_FALSE(wait_for_publisher(node_2, topic_name, std::chrono::seconds(1))); } @@ -181,11 +213,11 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) const std::string topic_name("test_always_automatic_liveliness"); // Create a publisher on domain 1 with liveliness set to "manual by topic" - auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_1 = domain_bridge::utils::create_node( "test_always_automatic_liveliness_node_1", domain_1_); rclcpp::QoS qos(1); - qos.liveliness(rclcpp::LivelinessPolicy::ManualByTopic); + qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); auto pub = node_1->create_publisher(topic_name, qos); // Bridge the publisher topic to domain 2 @@ -193,7 +225,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_2 = domain_bridge::utils::create_node( "test_always_automatic_liveliness_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); @@ -202,7 +234,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); - EXPECT_EQ(bridged_qos.liveliness(), rclcpp::LivelinessPolicy::Automatic); + rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); + EXPECT_EQ(rmw_bridged_qos_profile.liveliness, RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); } TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) @@ -212,7 +245,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) // Create two publishers on domain 1 // The first deadline will be greater than the second deadline // The second lifespan will be greater than the first lifespan - auto node_1 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_1 = domain_bridge::utils::create_node( "test_max_of_duration_policy_node_1", domain_1_); rclcpp::QoS qos_1(1); qos_1.deadline(rclcpp::Duration(554, 321u)) @@ -234,7 +267,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_, bridge_options); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node_with_name_and_domain_id( + auto node_2 = domain_bridge::utils::create_node( "test_max_of_duration_policy_node_2", domain_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); @@ -243,6 +276,11 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); - EXPECT_EQ(bridged_qos.deadline(), rclcpp::Duration(554, 321u)); - EXPECT_EQ(bridged_qos.lifespan(), rclcpp::Duration(554, 321u)); + rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.deadline), + rclcpp::Duration(554, 321u)); + EXPECT_EQ( + domain_bridge::utils::from_rmw_time(rmw_bridged_qos_profile.lifespan), + rclcpp::Duration(554, 321u)); } From 2a0c60d5329317eb175054a2e7ef9907bc140eb2 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Sat, 30 Oct 2021 17:04:18 -0400 Subject: [PATCH 17/28] Uncrustify fix in tests Signed-off-by: Abrar Rahman Protyasha --- test/domain_bridge/test_qos_matching.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index 03acc20..26f3a31 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -40,7 +40,7 @@ class TestDomainBridgeQosMatching : public ::testing::Test constexpr std::size_t TestDomainBridgeQosMatching::domain_1_; constexpr std::size_t TestDomainBridgeQosMatching::domain_2_; -std::ostream& operator<<(std::ostream& os, const rclcpp::TopicEndpointInfo& t) +std::ostream & operator<<(std::ostream & os, const rclcpp::TopicEndpointInfo & t) { os << t.node_namespace() << '/' << t.node_name() << ": " << t.topic_type(); return os; From efa97ae33fed98103a5030cf70e605dc7295ea1a Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 4 Nov 2021 01:37:28 -0400 Subject: [PATCH 18/28] Use `rcl_init_options_[set/get]_domain_id` in util Signed-off-by: Abrar Rahman Protyasha --- CMakeLists.txt | 4 +- include/domain_bridge/utils.hpp | 6 ++- package.xml | 6 +-- src/domain_bridge/utils.cpp | 66 ++++++++++++++++++++------------- 4 files changed, 51 insertions(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 43b2da5..b479ce8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) # Leverage rosbag2's generic type support utilities find_package(rosbag2_cpp REQUIRED) @@ -57,6 +58,7 @@ endif() ament_target_dependencies(${PROJECT_NAME}_lib rcl rclcpp + rcpputils rcutils rosbag2_cpp rosidl_typesupport_cpp @@ -82,7 +84,6 @@ target_link_libraries(${PROJECT_NAME}_exec # Only needed to build domain_bridge_rti_qos executable. find_package(rti_connext_dds_cmake_module QUIET) -find_package(rcpputils QUIET) set(executables ${PROJECT_NAME}_exec) @@ -131,6 +132,7 @@ ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( rcl rclcpp + rcpputils rcutils rosbag2_cpp rmw diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp index 5a145f9..f74d156 100644 --- a/include/domain_bridge/utils.hpp +++ b/include/domain_bridge/utils.hpp @@ -33,6 +33,10 @@ namespace utils DOMAIN_BRIDGE_PUBLIC rclcpp::Duration from_rmw_time(rmw_time_t duration); +DOMAIN_BRIDGE_PUBLIC +std::shared_ptr +create_context_with_domain_id(std::size_t domain_id); + DOMAIN_BRIDGE_PUBLIC rclcpp::Node::SharedPtr create_node( @@ -42,7 +46,7 @@ create_node( DOMAIN_BRIDGE_PUBLIC std::size_t -get_domain_id_from_node( +get_node_domain_id( rclcpp::Node & node); } // namespace utils diff --git a/package.xml b/package.xml index e956537..3a87d06 100644 --- a/package.xml +++ b/package.xml @@ -10,11 +10,11 @@ ament_cmake rosidl_default_generators - rcl - rmw - + rcl rclcpp rcutils + rcpputils + rmw rosbag2_cpp rosidl_typesupport_cpp yaml-cpp diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 4254f33..43eef96 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -17,6 +17,8 @@ #include #include +#include "rcl/allocator.h" +#include "rcl/init_options.h" #include "rcl/node.h" #include "rcl/node_options.h" #include "rcl/time.h" @@ -47,6 +49,30 @@ rclcpp::Duration from_rmw_time(rmw_time_t duration) return rclcpp::Duration{static_cast(total_ns)}; } +/// THROWS +std::shared_ptr +create_context_with_domain_id(std::size_t domain_id) +{ + rcl_init_options_t rcl_init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init( + &rcl_init_options, rcl_get_default_allocator()); + if (RCL_RET_OK != ret) { + std::runtime_error("Failed to initialize rcl_init_options"); + } + + ret = rcl_init_options_set_domain_id(&rcl_init_options, domain_id); + if (RCL_RET_OK != ret) { + std::runtime_error("Failed to set domain ID to rcl_init_options"); + } + + rclcpp::InitOptions init_options(rcl_init_options); + init_options.auto_initialize_logging(false); + + auto context = std::make_shared(); + context->init(0, nullptr, init_options); + return context; +} + rclcpp::Node::SharedPtr create_node( const std::string & name, @@ -54,11 +80,8 @@ create_node( std::shared_ptr context) { if (context == nullptr) { - context = std::make_shared(); + context = create_context_with_domain_id(domain_id); } - rclcpp::InitOptions init_options; - init_options.auto_initialize_logging(false); - context->init(0, nullptr, init_options); rclcpp::NodeOptions node_options; node_options.context(context) @@ -66,36 +89,27 @@ create_node( .start_parameter_services(false) .start_parameter_event_publisher(false); - auto node = std::make_shared(name, node_options); - auto node_base_interface = node->get_node_base_interface(); - rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); - // Hacky work-around because setting domain ID is not a feature in the rclcpp layer - const_cast( - rcl_node_get_options(rcl_node_handle))->domain_id = domain_id; - - return node; + return std::make_shared(name, node_options); } std::size_t -get_domain_id_from_node( +get_node_domain_id( rclcpp::Node & node) { - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface = - node.get_node_base_interface(); - rcl_node_t * rcl_node_handle = node_base_interface->get_rcl_node_handle(); + const rcl_init_options_t * rcl_init_options = + node.get_node_base_interface()->get_context()->get_init_options().get_rcl_init_options(); std::size_t domain_id; - rcl_ret_t ret = rcl_node_get_domain_id(rcl_node_handle, &domain_id); - if (ret == RCL_RET_OK) { - return domain_id; - } else if (ret == RCL_RET_NODE_INVALID) { - throw std::runtime_error("[get_domain_id_from_node] Node invalid."); - } else if (ret == RCL_RET_INVALID_ARGUMENT) { - throw std::runtime_error("[get_domain_id_from_node] Invalid argument."); - } else { - throw std::runtime_error("[get_domain_id_from_node] Unspecified error."); + // const_cast is safe because `rcl_init_options_get_domain_id` only reads the input structure + rcl_ret_t ret = + rcl_init_options_get_domain_id( + const_cast(rcl_init_options), + &domain_id); + if (RCL_RET_OK != ret) { + throw std::runtime_error("Failed to get domain ID from rcl_init_options"); } - // return (node.get_node_options().get_rcl_node_options())->domain_id; + + return domain_id; } } // namespace utils From 0153bf2866fb6cdcc59961708c76849bee317e79 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 4 Nov 2021 01:43:55 -0400 Subject: [PATCH 19/28] Ensure sources are compiling with new utility Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/wait_for_graph_events.hpp | 4 +- .../test_domain_bridge_end_to_end.cpp | 11 +++- .../test_domain_bridge_services.cpp | 2 +- test/domain_bridge/test_qos_matching.cpp | 66 ++++++++++++------- 4 files changed, 54 insertions(+), 29 deletions(-) diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index bf363b0..f6f1983 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -248,7 +248,7 @@ class WaitForGraphEvents // and print a warning if (reliable_count > 0u && reliable_count != num_endpoints) { result_qos.qos.best_effort(); - const size_t domain_id = domain_bridge::utils::get_domain_id_from_node(node); + const size_t domain_id = domain_bridge::utils::get_node_domain_id(node); std::string warning = "Some, but not all, publishers on topic '" + topic + "' on domain ID " + std::to_string(domain_id) + " offer 'reliable' reliability. Falling back to 'best effort' reliability in order " @@ -260,7 +260,7 @@ class WaitForGraphEvents // and print a warning if (transient_local_count > 0u && transient_local_count != num_endpoints) { result_qos.qos.durability_volatile(); - const size_t domain_id = domain_bridge::utils::get_domain_id_from_node(node); + const size_t domain_id = domain_bridge::utils::get_node_domain_id(node); std::string warning = "Some, but not all, publishers on topic '" + topic + "' on domain ID " + std::to_string(domain_id) + " offer 'transient local' durability. Falling back to 'volatile' durability in order " diff --git a/test/domain_bridge/test_domain_bridge_end_to_end.cpp b/test/domain_bridge/test_domain_bridge_end_to_end.cpp index e468018..c06983f 100644 --- a/test/domain_bridge/test_domain_bridge_end_to_end.cpp +++ b/test/domain_bridge/test_domain_bridge_end_to_end.cpp @@ -41,13 +41,15 @@ class TestDomainBridgeEndToEnd : public ::testing::Test protected: void SetUp() override { - context_1_ = std::make_shared(); + context_1_ = domain_bridge::utils::create_context_with_domain_id(kDomain1); node_1_ = domain_bridge::utils::create_node( "node_1", kDomain1, context_1_); + context_2_ = domain_bridge::utils::create_context_with_domain_id(kDomain2); node_2_ = domain_bridge::utils::create_node( - "node_2", kDomain2); + "node_2", kDomain2, context_2_); } std::shared_ptr context_1_; + std::shared_ptr context_2_; std::shared_ptr node_1_; std::shared_ptr node_2_; // use a qos profile with transient local volatility to make the tests simple. @@ -70,6 +72,11 @@ poll_condition(std::function condition, std::chrono::seconds timeout) return condition(); } +/* + * Can't call spin_until_future_complete() inside a callback executed by an executor. + * + * https://github.com/ros2/rclcpp/blob/d7804e1b3fd9676d302ec72f02c49ba04cbed5e6/rclcpp/include/rclcpp/executor.hpp#L198-L199 + */ class ScopedAsyncSpinner { public: diff --git a/test/domain_bridge/test_domain_bridge_services.cpp b/test/domain_bridge/test_domain_bridge_services.cpp index 8c67a23..1caf76b 100644 --- a/test/domain_bridge/test_domain_bridge_services.cpp +++ b/test/domain_bridge/test_domain_bridge_services.cpp @@ -39,7 +39,7 @@ class TestDomainBridgeServices : public ::testing::Test protected: void SetUp() override { - context_1_ = std::make_shared(); + context_1_ = domain_bridge::utils::create_context_with_domain_id(kDomain1); // Initialize one node in each domain node_1_ = domain_bridge::utils::create_node( "node_1", kDomain1, context_1_); diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index 26f3a31..fff235c 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -33,12 +33,30 @@ class TestDomainBridgeQosMatching : public ::testing::Test { protected: + static void SetUpTestCase() + { + // Initialize contexts in different domains + context_1_ = domain_bridge::utils::create_context_with_domain_id(domain_1_); + context_2_ = domain_bridge::utils::create_context_with_domain_id(domain_2_); + + node_options_1_.context(context_1_); + node_options_2_.context(context_2_); + } + static constexpr std::size_t domain_1_{1u}; static constexpr std::size_t domain_2_{2u}; + static std::shared_ptr context_1_; + static std::shared_ptr context_2_; + static rclcpp::NodeOptions node_options_1_; + static rclcpp::NodeOptions node_options_2_; }; constexpr std::size_t TestDomainBridgeQosMatching::domain_1_; constexpr std::size_t TestDomainBridgeQosMatching::domain_2_; +std::shared_ptr TestDomainBridgeQosMatching::context_1_; +std::shared_ptr TestDomainBridgeQosMatching::context_2_; +rclcpp::NodeOptions TestDomainBridgeQosMatching::node_options_1_; +rclcpp::NodeOptions TestDomainBridgeQosMatching::node_options_2_; std::ostream & operator<<(std::ostream & os, const rclcpp::TopicEndpointInfo & t) { @@ -51,8 +69,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) const std::string topic_name("test_topic_exists_before_bridge"); // Create a publisher on domain 1 - auto node_1 = domain_bridge::utils::create_node( - "test_topic_exists_before_bridge_node_1", domain_1_); + auto node_1 = std::make_shared( + "test_topic_exists_before_bridge_node_1", node_options_1_); rclcpp::QoS qos(1); qos.best_effort() .transient_local() @@ -66,8 +84,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node( - "test_topic_exists_before_bridge_node_2", domain_2_); + auto node_2 = std::make_shared( + "test_topic_exists_before_bridge_node_2", node_options_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the QoS of the bridged publisher matches @@ -76,8 +94,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) for (auto & t : endpoint_info_vec) { std::cout << t << '\n'; } - EXPECT_EQ(domain_bridge::utils::get_domain_id_from_node(*node_1), domain_1_); - EXPECT_EQ(domain_bridge::utils::get_domain_id_from_node(*node_2), domain_2_); + EXPECT_EQ(domain_bridge::utils::get_node_domain_id(*node_1), domain_1_); + EXPECT_EQ(domain_bridge::utils::get_node_domain_id(*node_2), domain_2_); EXPECT_EQ(endpoint_info_vec.size(), 1u); // ASSERT_EQ(endpoint_info_vec.size(), 1u); const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); @@ -100,8 +118,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) { const std::string topic_name("test_topic_exists_after_bridge"); - auto node_1 = domain_bridge::utils::create_node( - "test_topic_exists_after_bridge_node_1", domain_1_); + auto node_1 = std::make_shared( + "test_topic_exists_after_bridge_node_1", node_options_1_); // Bridge the publisher topic to domain 2 domain_bridge::DomainBridge bridge; @@ -109,8 +127,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) // Wait for bridge publisher to appear on domain 2 // It shouldn't be available yet - auto node_2 = domain_bridge::utils::create_node( - "test_topic_exists_after_bridge_node_2", domain_2_); + auto node_2 = std::make_shared( + "test_topic_exists_after_bridge_node_2", node_options_2_); ASSERT_FALSE(wait_for_publisher(node_2, topic_name, std::chrono::milliseconds(300))); // Create a publisher on domain 1 @@ -151,8 +169,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers const std::string topic_name("test_topic_exists_multiple_publishers"); // Create two publishers on domain 1 - auto node_1 = domain_bridge::utils::create_node( - "test_topic_exists_multiple_publishers_node_1", domain_1_); + auto node_1 = std::make_shared( + "test_topic_exists_multiple_publishers_node_1", node_options_1_); rclcpp::QoS qos(1); qos.reliable() .durability_volatile() @@ -169,8 +187,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node( - "test_topic_exists_multiple_publishers_node_2", domain_2_); + auto node_2 = std::make_shared( + "test_topic_exists_multiple_publishers_node_2", node_options_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the QoS of the bridged publishers matches both publishers @@ -203,8 +221,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_does_not_exist) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // We do not expect a bridge publisher to appear because there is no input publisher - auto node_2 = domain_bridge::utils::create_node( - "test_topic_does_not_exist_node_2", domain_2_); + auto node_2 = std::make_shared( + "test_topic_does_not_exist_node_2", node_options_2_); ASSERT_FALSE(wait_for_publisher(node_2, topic_name, std::chrono::seconds(1))); } @@ -213,8 +231,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) const std::string topic_name("test_always_automatic_liveliness"); // Create a publisher on domain 1 with liveliness set to "manual by topic" - auto node_1 = domain_bridge::utils::create_node( - "test_always_automatic_liveliness_node_1", domain_1_); + auto node_1 = std::make_shared( + "test_always_automatic_liveliness_node_1", node_options_1_); rclcpp::QoS qos(1); qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); @@ -225,8 +243,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) bridge.bridge_topic(topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node( - "test_always_automatic_liveliness_node_2", domain_2_); + auto node_2 = std::make_shared( + "test_always_automatic_liveliness_node_2", node_options_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert the liveliness policy is "automatic", not "manual by topic" @@ -245,8 +263,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) // Create two publishers on domain 1 // The first deadline will be greater than the second deadline // The second lifespan will be greater than the first lifespan - auto node_1 = domain_bridge::utils::create_node( - "test_max_of_duration_policy_node_1", domain_1_); + auto node_1 = std::make_shared( + "test_max_of_duration_policy_node_1", node_options_1_); rclcpp::QoS qos_1(1); qos_1.deadline(rclcpp::Duration(554, 321u)) .lifespan(rclcpp::Duration(123, 456u)); @@ -267,8 +285,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) topic_name, "test_msgs/msg/BasicTypes", domain_1_, domain_2_, bridge_options); // Wait for bridge publisher to appear on domain 2 - auto node_2 = domain_bridge::utils::create_node( - "test_max_of_duration_policy_node_2", domain_2_); + auto node_2 = std::make_shared( + "test_max_of_duration_policy_node_2", node_options_2_); ASSERT_TRUE(wait_for_publisher(node_2, topic_name)); // Assert max of the two deadline and lifespan policies are used for the bridge QoS From e29b8644f323b7307e0424bcd385c61d67acf8f3 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Tue, 31 May 2022 19:35:35 -0400 Subject: [PATCH 20/28] Fix getting domain ID process Reference: https://github.com/ros2/rclcpp/issues/910#issuecomment-1142629138 Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/utils.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 43eef96..303385d 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -96,8 +96,9 @@ std::size_t get_node_domain_id( rclcpp::Node & node) { - const rcl_init_options_t * rcl_init_options = - node.get_node_base_interface()->get_context()->get_init_options().get_rcl_init_options(); + const rclcpp::InitOptions init_options = + node.get_node_base_interface()->get_context()->get_init_options(); + const rcl_init_options_t * rcl_init_options = init_options.get_rcl_init_options(); std::size_t domain_id; // const_cast is safe because `rcl_init_options_get_domain_id` only reads the input structure From 6460754af2920a05d451fea141e522490283728b Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Tue, 31 May 2022 19:37:27 -0400 Subject: [PATCH 21/28] Ensure correct callback is being triggered Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/generic_subscription.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/domain_bridge/generic_subscription.cpp b/src/domain_bridge/generic_subscription.cpp index 55caaaf..670bb66 100644 --- a/src/domain_bridge/generic_subscription.cpp +++ b/src/domain_bridge/generic_subscription.cpp @@ -67,9 +67,9 @@ std::shared_ptr GenericSubscription::create_serialize 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"}; + auto typed_message = std::static_pointer_cast(message); + callback_(typed_message); } void GenericSubscription::handle_loaned_message( @@ -84,9 +84,9 @@ 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) serialized_message; + (void) message_info; + throw std::runtime_error{"unexpected callback being called"}; } void GenericSubscription::return_message(std::shared_ptr & message) From ff41c314d0e82b8cc864159e291756ab4f844f92 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Tue, 31 May 2022 19:37:55 -0400 Subject: [PATCH 22/28] Pass rcl_serialized_msg pointer properly Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/domain_bridge.cpp | 17 ++++++++++------- src/domain_bridge/generic_publisher.cpp | 4 ++-- src/domain_bridge/generic_publisher.hpp | 2 +- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 07b74fc..9c4bc77 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -191,7 +191,9 @@ class DomainBridgeImpl compressed_msg.data = domain_bridge::compress_message(cctx, std::move(*msg)); rclcpp::SerializedMessage serialized_compressed_msg; serializer.serialize_message(&compressed_msg, &serialized_compressed_msg); - publisher->publish(serialized_compressed_msg.get_rcl_serialized_message()); + auto serialized_data_ptr = std::make_shared( + serialized_compressed_msg.get_rcl_serialized_message()); + publisher->publish(serialized_data_ptr); }; break; case DomainBridgeOptions::Mode::Decompress: @@ -206,14 +208,18 @@ class DomainBridgeImpl serializer.deserialize_message(serialized_compressed_msg.get(), &compressed_msg); rclcpp::SerializedMessage msg = domain_bridge::decompress_message( dctx, std::move(compressed_msg.data)); - publisher->publish(msg.get_rcl_serialized_message()); + auto serialized_data_ptr = std::make_shared( + msg.get_rcl_serialized_message()); + publisher->publish(serialized_data_ptr); }; break; default: // fallthrough case DomainBridgeOptions::Mode::Normal: callback = [publisher](std::shared_ptr msg) { // Publish message into the other domain - publisher->publish(msg->get_rcl_serialized_message()); + auto serialized_data_ptr = std::make_shared( + msg->get_rcl_serialized_message()); + publisher->publish(serialized_data_ptr); }; break; } @@ -233,10 +239,7 @@ class DomainBridgeImpl *rosidl_typesupport_cpp::get_message_type_support_handle(), topic_name, qos, - [publisher](std::shared_ptr msg) { - // Publish message into the other domain - publisher->publish(msg->get_rcl_serialized_message()); - }); + callback); node->get_node_topics_interface()->add_subscription(subscription, std::move(group)); return subscription; } diff --git a/src/domain_bridge/generic_publisher.cpp b/src/domain_bridge/generic_publisher.cpp index ad55a81..d6830cf 100644 --- a/src/domain_bridge/generic_publisher.cpp +++ b/src/domain_bridge/generic_publisher.cpp @@ -42,10 +42,10 @@ GenericPublisher::GenericPublisher( : rclcpp::PublisherBase(node_base, topic_name, type_support, rosbag2_get_publisher_options(qos)) {} -void GenericPublisher::publish(const rmw_serialized_message_t & message) +void GenericPublisher::publish(std::shared_ptr message) { auto return_code = rcl_publish_serialized_message( - get_publisher_handle().get(), &message, nullptr); + 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"); diff --git a/src/domain_bridge/generic_publisher.hpp b/src/domain_bridge/generic_publisher.hpp index 9a5d85f..6f3d6ec 100644 --- a/src/domain_bridge/generic_publisher.hpp +++ b/src/domain_bridge/generic_publisher.hpp @@ -38,7 +38,7 @@ class GenericPublisher : public rclcpp::PublisherBase virtual ~GenericPublisher() = default; - void publish(const rmw_serialized_message_t & message); + void publish(std::shared_ptr message); }; } // namespace domain_bridge From d5c0c788eb2c930c66569fb0091355eab172862a Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 1 Jun 2022 03:12:35 -0400 Subject: [PATCH 23/28] Add docblocks for util functions Signed-off-by: Abrar Rahman Protyasha --- include/domain_bridge/utils.hpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp index f74d156..680bd35 100644 --- a/include/domain_bridge/utils.hpp +++ b/include/domain_bridge/utils.hpp @@ -30,13 +30,30 @@ namespace domain_bridge namespace utils { +/// Convert a duration represented as rmw_time_t to rclcpp::Duration. +/** + * \param duration: The duration to be converted. + * \return The provided duration as an rclcpp::Duration instance. + */ DOMAIN_BRIDGE_PUBLIC rclcpp::Duration from_rmw_time(rmw_time_t duration); +/// Create a ROS context with a given domain ID. +/** + * \param domain_id: The integral domain ID for the context. + * \return Shared pointer to an instance of a context with the given domain ID. + */ DOMAIN_BRIDGE_PUBLIC std::shared_ptr create_context_with_domain_id(std::size_t domain_id); +/// Create a ROS node with a given name, domain ID, and context instance. +/** + * \param name: The name of the generated node. + * \param context: The context instance (if not null) to which this node should belong. + * \param domain_id: The integral domain ID for the node. + * \return Shared pointer to an instance of a node with the given constraints. + */ DOMAIN_BRIDGE_PUBLIC rclcpp::Node::SharedPtr create_node( @@ -44,6 +61,11 @@ create_node( std::size_t domain_id, std::shared_ptr context = nullptr); +/// Get the domain ID of a ROS node. +/** + * \param node: The node whose domain ID will be obtained. + * \return The integral domain ID for the argument node. + */ DOMAIN_BRIDGE_PUBLIC std::size_t get_node_domain_id( From 2df39696d3b9193fa79eeb99d7cf07646da6b36e Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 1 Jun 2022 03:13:02 -0400 Subject: [PATCH 24/28] Change to ASSERT_EQ and improve test readability Signed-off-by: Abrar Rahman Protyasha --- test/domain_bridge/test_qos_matching.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index fff235c..0f5757c 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -91,13 +91,8 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) // Assert the QoS of the bridged publisher matches std::vector endpoint_info_vec = node_2->get_publishers_info_by_topic(topic_name); - for (auto & t : endpoint_info_vec) { - std::cout << t << '\n'; - } - EXPECT_EQ(domain_bridge::utils::get_node_domain_id(*node_1), domain_1_); - EXPECT_EQ(domain_bridge::utils::get_node_domain_id(*node_2), domain_2_); - EXPECT_EQ(endpoint_info_vec.size(), 1u); - // ASSERT_EQ(endpoint_info_vec.size(), 1u); + ASSERT_EQ(endpoint_info_vec.size(), 1u); + const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); rmw_qos_profile_t rmw_qos_profile = qos.get_rmw_qos_profile(); @@ -148,6 +143,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_after_bridge) std::vector endpoint_info_vec = node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); + const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); rmw_qos_profile_t rmw_qos_profile = qos.get_rmw_qos_profile(); @@ -196,6 +192,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers std::vector endpoint_info_vec = node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); + const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); rmw_qos_profile_t rmw_qos_profile = qos.get_rmw_qos_profile(); @@ -251,6 +248,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_always_automatic_liveliness) std::vector endpoint_info_vec = node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); + const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); EXPECT_EQ(rmw_bridged_qos_profile.liveliness, RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); @@ -293,6 +291,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_max_of_duration_policy) std::vector endpoint_info_vec = node_2->get_publishers_info_by_topic(topic_name); ASSERT_EQ(endpoint_info_vec.size(), 1u); + const rclcpp::QoS & bridged_qos = endpoint_info_vec[0].qos_profile(); rmw_qos_profile_t rmw_bridged_qos_profile = bridged_qos.get_rmw_qos_profile(); EXPECT_EQ( From 4bf6b8da32cfe7b36a5b2335b971d370f7f28cf0 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 1 Jun 2022 03:14:27 -0400 Subject: [PATCH 25/28] Instill const-correctness in util code Signed-off-by: Abrar Rahman Protyasha --- include/domain_bridge/utils.hpp | 8 ++++---- src/domain_bridge/utils.cpp | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp index 680bd35..c6a23dc 100644 --- a/include/domain_bridge/utils.hpp +++ b/include/domain_bridge/utils.hpp @@ -36,7 +36,7 @@ namespace utils * \return The provided duration as an rclcpp::Duration instance. */ DOMAIN_BRIDGE_PUBLIC -rclcpp::Duration from_rmw_time(rmw_time_t duration); +rclcpp::Duration from_rmw_time(const rmw_time_t duration); /// Create a ROS context with a given domain ID. /** @@ -44,8 +44,8 @@ rclcpp::Duration from_rmw_time(rmw_time_t duration); * \return Shared pointer to an instance of a context with the given domain ID. */ DOMAIN_BRIDGE_PUBLIC -std::shared_ptr -create_context_with_domain_id(std::size_t domain_id); +rclcpp::Context::SharedPtr +create_context_with_domain_id(const std::size_t domain_id); /// Create a ROS node with a given name, domain ID, and context instance. /** @@ -58,8 +58,8 @@ DOMAIN_BRIDGE_PUBLIC rclcpp::Node::SharedPtr create_node( const std::string & name, - std::size_t domain_id, std::shared_ptr context = nullptr); + const std::size_t domain_id, /// Get the domain ID of a ROS node. /** diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 303385d..6272423 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -35,23 +35,22 @@ namespace domain_bridge namespace utils { -rclcpp::Duration from_rmw_time(rmw_time_t duration) +rclcpp::Duration from_rmw_time(const rmw_time_t duration) { constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); if (duration.sec > limit_sec || duration.nsec > limit_ns) { return rclcpp::Duration{limit_ns}; } - uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; + const uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; if (total_ns > limit_ns) { return rclcpp::Duration{limit_ns}; } return rclcpp::Duration{static_cast(total_ns)}; } -/// THROWS std::shared_ptr -create_context_with_domain_id(std::size_t domain_id) +create_context_with_domain_id(const std::size_t domain_id) { rcl_init_options_t rcl_init_options = rcl_get_zero_initialized_init_options(); rcl_ret_t ret = rcl_init_options_init( @@ -76,8 +75,8 @@ create_context_with_domain_id(std::size_t domain_id) rclcpp::Node::SharedPtr create_node( const std::string & name, - std::size_t domain_id, std::shared_ptr context) + const std::size_t domain_id, { if (context == nullptr) { context = create_context_with_domain_id(domain_id); @@ -96,13 +95,14 @@ std::size_t get_node_domain_id( rclcpp::Node & node) { + // Need separate instance for InitOptions so that it stays in scope till domain ID is obtained const rclcpp::InitOptions init_options = node.get_node_base_interface()->get_context()->get_init_options(); const rcl_init_options_t * rcl_init_options = init_options.get_rcl_init_options(); std::size_t domain_id; // const_cast is safe because `rcl_init_options_get_domain_id` only reads the input structure - rcl_ret_t ret = + const rcl_ret_t ret = rcl_init_options_get_domain_id( const_cast(rcl_init_options), &domain_id); From b93f9e053746d3c30fc032a64ebb9feb08d6b4e5 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 1 Jun 2022 03:14:55 -0400 Subject: [PATCH 26/28] Convert shared_ptr to T::SharedPtr Improves readability of code for rclcpp types, in my opinion. Signed-off-by: Abrar Rahman Protyasha --- include/domain_bridge/utils.hpp | 2 +- src/domain_bridge/utils.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp index c6a23dc..1321182 100644 --- a/include/domain_bridge/utils.hpp +++ b/include/domain_bridge/utils.hpp @@ -58,8 +58,8 @@ DOMAIN_BRIDGE_PUBLIC rclcpp::Node::SharedPtr create_node( const std::string & name, - std::shared_ptr context = nullptr); const std::size_t domain_id, + rclcpp::Context::SharedPtr context = nullptr); /// Get the domain ID of a ROS node. /** diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 6272423..061ba6a 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -49,7 +49,7 @@ rclcpp::Duration from_rmw_time(const rmw_time_t duration) return rclcpp::Duration{static_cast(total_ns)}; } -std::shared_ptr +rclcpp::Context::SharedPtr create_context_with_domain_id(const std::size_t domain_id) { rcl_init_options_t rcl_init_options = rcl_get_zero_initialized_init_options(); @@ -75,8 +75,8 @@ create_context_with_domain_id(const std::size_t domain_id) rclcpp::Node::SharedPtr create_node( const std::string & name, - std::shared_ptr context) const std::size_t domain_id, + rclcpp::Context::SharedPtr context) { if (context == nullptr) { context = create_context_with_domain_id(domain_id); From a91fbbdec64e890610407a7fe0be030eff8813a9 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 1 Jun 2022 03:15:26 -0400 Subject: [PATCH 27/28] Add from_rmw_time function reference Signed-off-by: Abrar Rahman Protyasha --- src/domain_bridge/utils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/domain_bridge/utils.cpp b/src/domain_bridge/utils.cpp index 061ba6a..e9ea8ba 100644 --- a/src/domain_bridge/utils.cpp +++ b/src/domain_bridge/utils.cpp @@ -37,6 +37,7 @@ namespace utils rclcpp::Duration from_rmw_time(const rmw_time_t duration) { + // From https://github.com/ros2/rclcpp/pull/1467/commits/b6bfc5868035b3cc8774275e08c6194b0f484e37 constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); if (duration.sec > limit_sec || duration.nsec > limit_ns) { From 4af13e7bd16483c0b807c6f88d01bb532a6c598b Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 1 Jun 2022 03:21:34 -0400 Subject: [PATCH 28/28] Remove unnecessary ostream op overload Signed-off-by: Abrar Rahman Protyasha --- test/domain_bridge/test_qos_matching.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/test/domain_bridge/test_qos_matching.cpp b/test/domain_bridge/test_qos_matching.cpp index 0f5757c..480ada6 100644 --- a/test/domain_bridge/test_qos_matching.cpp +++ b/test/domain_bridge/test_qos_matching.cpp @@ -58,12 +58,6 @@ std::shared_ptr TestDomainBridgeQosMatching::context_2_; rclcpp::NodeOptions TestDomainBridgeQosMatching::node_options_1_; rclcpp::NodeOptions TestDomainBridgeQosMatching::node_options_2_; -std::ostream & operator<<(std::ostream & os, const rclcpp::TopicEndpointInfo & t) -{ - os << t.node_namespace() << '/' << t.node_name() << ": " << t.topic_type(); - return os; -} - TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_bridge) { const std::string topic_name("test_topic_exists_before_bridge");