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 diff --git a/CMakeLists.txt b/CMakeLists.txt index d6f850b..b479ce8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,12 +16,15 @@ 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(rcpputils REQUIRED) find_package(rcutils REQUIRED) # Leverage rosbag2's generic type support utilities 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) @@ -34,10 +37,13 @@ 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 src/${PROJECT_NAME}/topic_bridge_options.cpp + src/${PROJECT_NAME}/utils.cpp ) target_include_directories(${PROJECT_NAME}_lib PUBLIC @@ -50,10 +56,13 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif() ament_target_dependencies(${PROJECT_NAME}_lib + rcl rclcpp + rcpputils rcutils rosbag2_cpp rosidl_typesupport_cpp + rmw yaml_cpp_vendor zstd ) @@ -75,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) @@ -122,9 +130,12 @@ install(DIRECTORY examples launch ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( + rcl rclcpp - rosbag2_cpp + rcpputils rcutils + rosbag2_cpp + rmw yaml_cpp_vendor zstd_vendor zstd 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/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/include/domain_bridge/utils.hpp b/include/domain_bridge/utils.hpp new file mode 100644 index 0000000..1321182 --- /dev/null +++ b/include/domain_bridge/utils.hpp @@ -0,0 +1,78 @@ +// 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 "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "rmw/types.h" + +#include "domain_bridge/visibility_control.hpp" + +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(const 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 +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. +/** + * \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( + const std::string & name, + const std::size_t domain_id, + rclcpp::Context::SharedPtr 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( + rclcpp::Node & node); + +} // namespace utils + +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__UTILS_HPP_ diff --git a/package.xml b/package.xml index d9fd5bd..3a87d06 100644 --- a/package.xml +++ b/package.xml @@ -10,8 +10,11 @@ ament_cmake rosidl_default_generators + rcl rclcpp rcutils + rcpputils + rmw rosbag2_cpp rosidl_typesupport_cpp yaml-cpp @@ -30,7 +33,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..9c4bc77 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" @@ -43,45 +41,15 @@ #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" 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 -{ -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_; -}; - /// Implementation of \ref DomainBridge. class DomainBridgeImpl { @@ -90,8 +58,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>>; @@ -118,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); @@ -145,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(oss.str(), domain_id); node_map_[domain_id] = node; return node; } @@ -195,32 +143,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 +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); + auto serialized_data_ptr = std::make_shared( + serialized_compressed_msg.get_rcl_serialized_message()); + publisher->publish(serialized_data_ptr); }; break; case DomainBridgeOptions::Mode::Decompress: @@ -251,31 +208,40 @@ 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); + 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); + auto serialized_data_ptr = std::make_shared( + msg->get_rcl_serialized_message()); + publisher->publish(serialized_data_ptr); }; 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); + callback); + node->get_node_topics_interface()->add_subscription(subscription, std::move(group)); + return subscription; } void bridge_topic( @@ -340,67 +306,68 @@ 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(); 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()); + 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(); 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()); + 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.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) { 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..d6830cf --- /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(std::shared_ptr message) +{ + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), message.get(), NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); + } +} + +} // namespace domain_bridge diff --git a/src/domain_bridge/generic_publisher.hpp b/src/domain_bridge/generic_publisher.hpp new file mode 100644 index 0000000..6f3d6ec --- /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(std::shared_ptr message); +}; + +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__GENERIC_PUBLISHER_HPP_ diff --git a/src/domain_bridge/generic_subscription.cpp b/src/domain_bridge/generic_subscription.cpp new file mode 100644 index 0000000..670bb66 --- /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_info; + auto typed_message = std::static_pointer_cast(message); + callback_(typed_message); +} + +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) serialized_message; + (void) message_info; + throw std::runtime_error{"unexpected callback being called"}; +} + +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..7100cc6 --- /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 all of Foxy, 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_ 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/utils.cpp b/src/domain_bridge/utils.cpp new file mode 100644 index 0000000..e9ea8ba --- /dev/null +++ b/src/domain_bridge/utils.cpp @@ -0,0 +1,119 @@ +// 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 +#include + +#include "rcl/allocator.h" +#include "rcl/init_options.h" +#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" + +namespace domain_bridge +{ + +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) { + return rclcpp::Duration{limit_ns}; + } + 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)}; +} + +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(); + 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, + const std::size_t domain_id, + rclcpp::Context::SharedPtr context) +{ + if (context == nullptr) { + context = create_context_with_domain_id(domain_id); + } + + rclcpp::NodeOptions node_options; + node_options.context(context) + .use_global_arguments(false) + .start_parameter_services(false) + .start_parameter_event_publisher(false); + + return std::make_shared(name, node_options); +} + +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 + const 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 domain_id; +} + +} // namespace utils + +} // namespace domain_bridge diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index 75c1485..f6f1983 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -28,9 +28,15 @@ #include #include +#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 "domain_bridge/utils.hpp" namespace domain_bridge { @@ -204,10 +210,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 @@ -217,18 +227,20 @@ 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) { - 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; } } @@ -236,8 +248,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 = 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(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 +260,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 = 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(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); 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..c06983f 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,25 +41,12 @@ 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); + 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, context_2_); } std::shared_ptr context_1_; std::shared_ptr context_2_; @@ -84,12 +72,17 @@ 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: 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 +129,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 +160,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 +191,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 +221,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 84be850..1caf76b 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,14 @@ 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); - + context_1_ = domain_bridge::utils::create_context_with_domain_id(kDomain1); // 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_; }; @@ -83,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 02de7fd..480ada6 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,9 +22,11 @@ #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" +#include "domain_bridge/utils.hpp" #include "wait_for_publisher.hpp" @@ -33,30 +36,23 @@ class TestDomainBridgeQosMatching : public ::testing::Test 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); + 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 const std::size_t domain_1_{1u}; - static const std::size_t domain_2_{2u}; + 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_; }; -const std::size_t TestDomainBridgeQosMatching::domain_1_; -const std::size_t TestDomainBridgeQosMatching::domain_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_; @@ -74,7 +70,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_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); // Bridge the publisher topic to domain 2 @@ -90,14 +86,21 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_before_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(); - 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::from_nanoseconds(std::numeric_limits::max()); - EXPECT_EQ(bridged_qos.deadline(), max_duration); - EXPECT_EQ(bridged_qos.lifespan(), max_duration); + auto max_duration = rclcpp::Duration(std::numeric_limits::max()); + 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) @@ -123,7 +126,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 @@ -134,14 +137,21 @@ 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(); - 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::from_nanoseconds(std::numeric_limits::max()); - EXPECT_EQ(bridged_qos.deadline(), max_duration); - EXPECT_EQ(bridged_qos.lifespan(), max_duration); + auto max_duration = rclcpp::Duration(std::numeric_limits::max()); + 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) @@ -156,7 +166,7 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_topic_exists_multiple_publishers .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(); @@ -176,14 +186,21 @@ 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(); - 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::from_nanoseconds(std::numeric_limits::max()); - EXPECT_EQ(bridged_qos.deadline(), max_duration); - EXPECT_EQ(bridged_qos.lifespan(), max_duration); + auto max_duration = rclcpp::Duration(std::numeric_limits::max()); + 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) @@ -207,8 +224,9 @@ TEST_F(TestDomainBridgeQosMatching, qos_matches_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_); + 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 @@ -224,8 +242,10 @@ 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(); - 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) @@ -265,7 +285,13 @@ 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(); - 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)); }