diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 79d67325f4..d28ce2be97 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -5,6 +5,7 @@ project(rclcpp) find_package(Threads REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(libstatistics_collector REQUIRED) find_package(rcl REQUIRED) @@ -51,12 +52,14 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp src/rclcpp/executors.cpp - src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/executors/multi_threaded_executor.cpp src/rclcpp/executors/single_threaded_executor.cpp src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp + src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/future_return_code.cpp + src/rclcpp/generic_publisher.cpp + src/rclcpp/generic_subscription.cpp src/rclcpp/graph_listener.cpp src/rclcpp/guard_condition.cpp src/rclcpp/init_options.cpp @@ -67,7 +70,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/memory_strategy.cpp src/rclcpp/message_info.cpp src/rclcpp/node.cpp - src/rclcpp/node_options.cpp src/rclcpp/node_interfaces/node_base.cpp src/rclcpp/node_interfaces/node_clock.cpp src/rclcpp/node_interfaces/node_graph.cpp @@ -78,13 +80,14 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp src/rclcpp/node_interfaces/node_waitables.cpp + src/rclcpp/node_options.cpp src/rclcpp/parameter.cpp - src/rclcpp/parameter_value.cpp src/rclcpp/parameter_client.cpp src/rclcpp/parameter_event_handler.cpp src/rclcpp/parameter_events_filter.cpp src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp + src/rclcpp/parameter_value.cpp src/rclcpp/publisher_base.cpp src/rclcpp/qos.cpp src/rclcpp/qos_event.cpp @@ -99,6 +102,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/time_source.cpp src/rclcpp/timer.cpp src/rclcpp/type_support.cpp + src/rclcpp/typesupport_helpers.cpp src/rclcpp/utilities.cpp src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp src/rclcpp/waitable.cpp @@ -179,6 +183,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} + "ament_index_cpp" "libstatistics_collector" "rcl" "rcl_yaml_param_parser" @@ -209,6 +214,7 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_targets(${PROJECT_NAME}) +ament_export_dependencies(ament_index_cpp) ament_export_dependencies(libstatistics_collector) ament_export_dependencies(rcl) ament_export_dependencies(rcpputils) diff --git a/rclcpp/include/rclcpp/create_generic_publisher.hpp b/rclcpp/include/rclcpp/create_generic_publisher.hpp new file mode 100644 index 0000000000..296446f7b9 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_publisher.hpp @@ -0,0 +1,69 @@ +// Copyright 2020, Apex.AI 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 RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ +#define RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ + +#include +#include +#include + +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +namespace rclcpp +{ + +/// Create and return a GenericPublisher. +/** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param topics_interface NodeTopicsInterface pointer used in parts of the setup + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ +template> +std::shared_ptr create_generic_publisher( + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + auto ts_lib = rclcpp::get_typesupport_library(topic_type, "rosidl_typesupport_cpp"); + auto pub = std::make_shared( + topics_interface->get_node_base_interface(), + std::move(ts_lib), + topic_name, + topic_type, + qos, + options); + topics_interface->add_publisher(pub, options.callback_group); + return pub; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/create_generic_subscription.hpp b/rclcpp/include/rclcpp/create_generic_subscription.hpp new file mode 100644 index 0000000000..f5281cc673 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_subscription.hpp @@ -0,0 +1,79 @@ +// Copyright 2020, Apex.AI 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 RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ +#define RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ + +#include +#include +#include +#include + +#include "rcl/subscription.h" +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +namespace rclcpp +{ + +/// Create and return a GenericSubscription. +/** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param topics_interface NodeTopicsInterface pointer used in parts of the setup. + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ +template> +std::shared_ptr create_generic_subscription( + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) +) +{ + auto ts_lib = rclcpp::get_typesupport_library( + topic_type, "rosidl_typesupport_cpp"); + + auto subscription = std::make_shared( + topics_interface->get_node_base_interface(), + std::move(ts_lib), + topic_name, + topic_type, + qos, + callback, + options); + + topics_interface->add_subscription(subscription, options.callback_group); + + return subscription; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp new file mode 100644 index 0000000000..7cadda5d1d --- /dev/null +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -0,0 +1,127 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 RCLCPP__GENERIC_PUBLISHER_HPP_ +#define RCLCPP__GENERIC_PUBLISHER_HPP_ + +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// %Publisher for serialized messages whose type is not known at compile time. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * It does not support intra-process handling. + */ +class GenericPublisher : public rclcpp::PublisherBase +{ +public: + // cppcheck-suppress unknownMacro + RCLCPP_SMART_PTR_DEFINITIONS(GenericPublisher) + + /// Constructor. + /** + * In order to properly publish to a topic, this publisher needs to be added to + * the node_topic_interface of the node passed into this constructor. + * + * \sa rclcpp::Node::create_generic_publisher() or rclcpp::create_generic_publisher() for + * creating an instance of this class and adding it to the node_topic_interface. + * + * \param node_base Pointer to parent node's NodeBaseInterface + * \param ts_lib Type support library, needs to correspond to topic_type + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ + template> + GenericPublisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + std::shared_ptr ts_lib, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + : rclcpp::PublisherBase( + node_base, + topic_name, + *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + options.template to_rcl_publisher_options(qos)), + ts_lib_(ts_lib) + { + // This is unfortunately duplicated with the code in publisher.hpp. + // TODO(nnmm): Deduplicate by moving this into PublisherBase. + if (options.event_callbacks.deadline_callback) { + this->add_event_handler( + options.event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (options.event_callbacks.liveliness_callback) { + this->add_event_handler( + options.event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + if (options.event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + options.event_callbacks.incompatible_qos_callback, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } else if (options.use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](QOSOfferedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass + } + } + } + + RCLCPP_PUBLIC + virtual ~GenericPublisher() = default; + + /// Publish a rclcpp::SerializedMessage. + RCLCPP_PUBLIC + void publish(const rclcpp::SerializedMessage & message); + +private: + // The type support library should stay loaded, so it is stored in the GenericPublisher + std::shared_ptr ts_lib_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp new file mode 100644 index 0000000000..d9f40d868b --- /dev/null +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -0,0 +1,161 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 RCLCPP__GENERIC_SUBSCRIPTION_HPP_ +#define RCLCPP__GENERIC_SUBSCRIPTION_HPP_ + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// %Subscription for serialized messages whose type is not known at compile time. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * 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. + * + * \sa rclcpp::Node::create_generic_subscription() or rclcpp::create_generic_subscription() for + * creating an instance of this class and adding it to the node_topic_interface. + * + * \param node_base Pointer to parent node's NodeBaseInterface + * \param ts_lib Type support library, needs to correspond to topic_type + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Subscription options. + * Not all subscription options are currently respected, the only relevant options for this + * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and + * `%callback_group`. + */ + template> + GenericSubscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::shared_ptr ts_lib, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + // TODO(nnmm): Add variant for callback with message info. See issue #1604. + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) + : SubscriptionBase( + node_base, + *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + topic_name, + options.template to_rcl_subscription_options(qos), + true), + callback_(callback), + ts_lib_(ts_lib) + { + // This is unfortunately duplicated with the code in subscription.hpp. + // TODO(nnmm): Deduplicate by moving this into SubscriptionBase. + if (options.event_callbacks.deadline_callback) { + this->add_event_handler( + options.event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (options.event_callbacks.liveliness_callback) { + this->add_event_handler( + options.event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + if (options.event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + options.event_callbacks.incompatible_qos_callback, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } else if (options.use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](QOSRequestedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass + } + } + if (options.event_callbacks.message_lost_callback) { + this->add_event_handler( + options.event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } + } + + RCLCPP_PUBLIC + virtual ~GenericSubscription() = default; + + // Same as create_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + std::shared_ptr create_message() override; + + RCLCPP_PUBLIC + std::shared_ptr create_serialized_message() override; + + /// Cast the message to a rclcpp::SerializedMessage and call the callback. + RCLCPP_PUBLIC + void handle_message( + std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override; + + /// This function is currently not implemented. + RCLCPP_PUBLIC + void handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) override; + + // Same as return_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + void return_message(std::shared_ptr & message) override; + + RCLCPP_PUBLIC + void return_serialized_message(std::shared_ptr & message) override; + +private: + RCLCPP_DISABLE_COPY(GenericSubscription) + + std::function)> callback_; + // The type support library should stay loaded, so it is stored in the GenericSubscription + std::shared_ptr ts_lib_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 78eb1db93b..b2156a0068 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -41,6 +42,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -266,6 +269,55 @@ class Node : public std::enable_shared_from_this const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. + /** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param[in] topic_name Topic name + * \param[in] topic_type Topic type + * \param[in] qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + * \return Shared pointer to the created generic publisher. + */ + template> + std::shared_ptr create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) + ); + + /// Create and return a GenericSubscription. + /** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param[in] topic_name Topic name + * \param[in] topic_type Topic type + * \param[in] qos %QoS settings + * \param[in] callback Callback for new messages of serialized form + * \param[in] options %Subscription options. + * Not all subscription options are currently respected, the only relevant options for this + * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and + * `%callback_group`. + * \return Shared pointer to the created generic subscription. + */ + template> + std::shared_ptr create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) + ); + /// Declare and initialize a parameter, return the effective value. /** * This method is used to declare that a parameter exists on this node. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 8ed078113f..48e2430541 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -36,10 +36,12 @@ #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/create_client.hpp" +#include "rclcpp/create_generic_publisher.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" -#include "rclcpp/create_timer.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp/detail/resolve_enable_topic_statistics.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" @@ -152,6 +154,43 @@ Node::create_service( group); } +template +std::shared_ptr +Node::create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_generic_publisher( + node_topics_, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + topic_type, + qos, + options + ); +} + +template +std::shared_ptr +Node::create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) +{ + return rclcpp::create_generic_subscription( + node_topics_, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + topic_type, + qos, + std::move(callback), + options + ); +} + + template auto Node::declare_parameter( diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 56e999f702..7f0498fe49 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -117,6 +117,15 @@ * - Allocator related items: * - rclcpp/allocator/allocator_common.hpp * - rclcpp/allocator/allocator_deleter.hpp + * - Generic publisher + * - rclcpp::Node::create_generic_publisher() + * - rclcpp::GenericPublisher + * - rclcpp::GenericPublisher::publish() + * - rclcpp/generic_publisher.hpp + * - Generic subscription + * - rclcpp::Node::create_generic_subscription() + * - rclcpp::GenericSubscription + * - rclcpp/generic_subscription.hpp * - Memory management tools: * - rclcpp/memory_strategies.hpp * - rclcpp/memory_strategy.hpp @@ -134,6 +143,7 @@ * - rclcpp/scope_exit.hpp * - rclcpp/time.hpp * - rclcpp/utilities.hpp + * - rclcpp/typesupport_helpers.hpp * - rclcpp/visibility_control.hpp */ diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 28b92ffb13..c9a3ec7b7c 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -61,8 +61,11 @@ class SubscriptionBase : public std::enable_shared_from_this public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) - /// Default constructor. + /// Constructor. /** + * This accepts rcl_subscription_options_t instead of rclcpp::SubscriptionOptions because + * rclcpp::SubscriptionOptions::to_rcl_subscription_options depends on the message type. + * * \param[in] node_base NodeBaseInterface pointer used in parts of the setup. * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. @@ -77,7 +80,7 @@ class SubscriptionBase : public std::enable_shared_from_this const rcl_subscription_options_t & subscription_options, bool is_serialized = false); - /// Default destructor. + /// Destructor. RCLCPP_PUBLIC virtual ~SubscriptionBase(); diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp new file mode 100644 index 0000000000..2fad84cf3b --- /dev/null +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -0,0 +1,57 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 RCLCPP__TYPESUPPORT_HELPERS_HPP_ +#define RCLCPP__TYPESUPPORT_HELPERS_HPP_ + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +/// Load the type support library for the given type. +/** + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \return A shared library + */ +RCLCPP_PUBLIC +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier); + +/// Extract the type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \return A type support handle + */ +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + +} // namespace rclcpp + +#endif // RCLCPP__TYPESUPPORT_HELPERS_HPP_ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index bb0977e7b0..c7709693d7 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -12,12 +12,14 @@ ament_cmake_ros + ament_index_cpp builtin_interfaces rcl_interfaces rosgraph_msgs rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp + ament_index_cpp builtin_interfaces rcl_interfaces rosgraph_msgs diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp new file mode 100644 index 0000000000..733aa5dd5c --- /dev/null +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -0,0 +1,34 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rclcpp/generic_publisher.hpp" + +#include +#include + +namespace rclcpp +{ + +void GenericPublisher::publish(const rclcpp::SerializedMessage & message) +{ + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp new file mode 100644 index 0000000000..d72d21a0f9 --- /dev/null +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -0,0 +1,67 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rclcpp/generic_subscription.hpp" + +#include +#include + +#include "rcl/subscription.h" + +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ + +std::shared_ptr GenericSubscription::create_message() +{ + return create_serialized_message(); +} + +std::shared_ptr GenericSubscription::create_serialized_message() +{ + return std::make_shared(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 rclcpp::exceptions::UnimplementedError( + "handle_loaned_message is not implemented for GenericSubscription"); +} + +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(); +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/typesupport_helpers.cpp b/rclcpp/src/rclcpp/typesupport_helpers.cpp new file mode 100644 index 0000000000..7286c35baa --- /dev/null +++ b/rclcpp/src/rclcpp/typesupport_helpers.cpp @@ -0,0 +1,136 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rclcpp/typesupport_helpers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "ament_index_cpp/get_package_prefix.hpp" +#include "ament_index_cpp/get_resources.hpp" +#include "rcpputils/shared_library.hpp" +#include "rcpputils/find_library.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" + +namespace rclcpp +{ + +namespace +{ + +// Look for the library in the ament prefix paths. +std::string get_typesupport_library_path( + const std::string & package_name, const std::string & typesupport_identifier) +{ + const char * dynamic_library_folder; +#ifdef _WIN32 + dynamic_library_folder = "/bin/"; +#elif __APPLE__ + dynamic_library_folder = "/lib/"; +#else + dynamic_library_folder = "/lib/"; +#endif + + std::string package_prefix; + try { + package_prefix = ament_index_cpp::get_package_prefix(package_name); + } catch (ament_index_cpp::PackageNotFoundError & e) { + throw std::runtime_error(e.what()); + } + + const std::string library_path = rcpputils::path_for_library( + package_prefix + dynamic_library_folder, + package_name + "__" + typesupport_identifier); + if (library_path.empty()) { + throw std::runtime_error( + "Typesupport library for " + package_name + " does not exist in '" + package_prefix + + "'."); + } + return library_path; +} + +std::tuple +extract_type_identifier(const std::string & full_type) +{ + char type_separator = '/'; + auto sep_position_back = full_type.find_last_of(type_separator); + auto sep_position_front = full_type.find_first_of(type_separator); + if (sep_position_back == std::string::npos || + sep_position_back == 0 || + sep_position_back == full_type.length() - 1) + { + throw std::runtime_error( + "Message type is not of the form package/type and cannot be processed"); + } + + std::string package_name = full_type.substr(0, sep_position_front); + std::string middle_module = ""; + if (sep_position_back - sep_position_front > 0) { + middle_module = + full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1); + } + std::string type_name = full_type.substr(sep_position_back + 1); + + return std::make_tuple(package_name, middle_module, type_name); +} + +} // anonymous namespace + +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier) +{ + auto package_name = std::get<0>(extract_type_identifier(type)); + auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); + return std::make_shared(library_path); +} + +const rosidl_message_type_support_t * +get_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + std::string package_name; + std::string middle_module; + std::string type_name; + std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); + + auto mk_error = [&package_name, &type_name](auto reason) { + std::stringstream rcutils_dynamic_loading_error; + rcutils_dynamic_loading_error << + "Something went wrong loading the typesupport library for message type " << package_name << + "/" << type_name << ". " << reason; + return rcutils_dynamic_loading_error.str(); + }; + + try { + std::string symbol_name = typesupport_identifier + "__get_message_type_support_handle__" + + package_name + "__" + (middle_module.empty() ? "msg" : middle_module) + "__" + type_name; + + const rosidl_message_type_support_t * (* get_ts)() = nullptr; + // This will throw runtime_error if the symbol was not found. + get_ts = reinterpret_cast(library.get_symbol(symbol_name)); + return get_ts(); + } catch (std::runtime_error &) { + throw std::runtime_error{mk_error("Library could not be found.")}; + } +} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d26c3ed07d..f6244b5815 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -368,6 +368,21 @@ if(TARGET test_qos) ${PROJECT_NAME} ) endif() +function(test_generic_pubsub_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp + ENV ${rmw_implementation_env_var} + ) + if(TARGET test_generic_pubsub${target_suffix}) + target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) + ament_target_dependencies(test_generic_pubsub${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) ament_add_gtest(test_qos_event test_qos_event.cpp) if(TARGET test_qos_event) ament_target_dependencies(test_qos_event @@ -473,6 +488,10 @@ if(TARGET test_type_support) ) target_link_libraries(test_type_support ${PROJECT_NAME}) endif() +ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) +if(TARGET test_typesupport_helpers) + target_link_libraries(test_typesupport_helpers ${PROJECT_NAME}) +endif() ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) ament_target_dependencies(test_find_weak_nodes diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp new file mode 100644 index 0000000000..71a60a77fb --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -0,0 +1,196 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 + +#include "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/basic_types.hpp" + +#include "rcl/graph.h" + +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" + +using namespace ::testing; // NOLINT +using namespace rclcpp; // NOLINT + +class RclcppGenericNodeFixture : public Test +{ +public: + RclcppGenericNodeFixture() + { + node_ = std::make_shared("pubsub"); + publisher_node_ = std::make_shared( + "publisher_node", + rclcpp::NodeOptions().start_parameter_event_publisher(false)); + } + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void create_publisher(const std::string & topic) + { + auto publisher = publisher_node_->create_publisher(topic, 10); + publishers_.push_back(publisher); + } + + std::vector subscribe_raw_messages( + size_t expected_messages_number, const std::string & topic_name, const std::string & type) + { + std::vector messages; + size_t counter = 0; + auto subscription = node_->create_generic_subscription( + topic_name, type, rclcpp::QoS(1), + [&counter, &messages](std::shared_ptr message) { + test_msgs::msg::Strings string_message; + rclcpp::Serialization serializer; + serializer.deserialize_message(message.get(), &string_message); + messages.push_back(string_message.string_value); + counter++; + }); + + while (counter < expected_messages_number) { + rclcpp::spin_some(node_); + } + return messages; + } + + rclcpp::SerializedMessage serialize_string_message(const std::string & message) + { + test_msgs::msg::Strings string_message; + string_message.string_value = message; + rclcpp::Serialization ser; + SerializedMessage result; + ser.serialize_message(&string_message, &result); + return result; + } + + void sleep_to_allow_topics_discovery() + { + // This is a short sleep to allow the node some time to discover the topic + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node_); + } + return true; + } + + std::shared_ptr node_; + rclcpp::Node::SharedPtr publisher_node_; + std::vector> publishers_; +}; + + +TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) +{ + // We currently publish more messages because they can get lost + std::vector test_messages = {"Hello World", "Hello World"}; + std::string topic_name = "/string_topic"; + std::string type = "test_msgs/msg/Strings"; + + auto publisher = node_->create_generic_publisher( + topic_name, type, rclcpp::QoS(1)); + + auto subscriber_future_ = std::async( + std::launch::async, [this, topic_name, type] { + return subscribe_raw_messages(1, topic_name, type); + }); + + // TODO(karsten1987): Port 'wait_for_sub' to rclcpp + auto allocator = node_->get_node_options().allocator(); + auto success = false; + auto ret = rcl_wait_for_subscribers( + node_->get_node_base_interface()->get_rcl_node_handle(), + &allocator, + topic_name.c_str(), + 1u, + static_cast(1e9), + &success); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(success); + + for (const auto & message : test_messages) { + publisher->publish(serialize_string_message(message)); + } + + auto subscribed_messages = subscriber_future_.get(); + EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); + EXPECT_THAT(subscribed_messages[0], StrEq("Hello World")); +} + +TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) +{ + // If the GenericSubscription does not use the provided QoS profile, + // its request will be incompatible with the Publisher's offer and no messages will be passed. + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::SensorDataQoS(); + + auto publisher = node_->create_publisher(topic_name, qos); + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); +} + +TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) +{ + // If the GenericPublisher does not use the provided QoS profile, + // its offer will be incompatible with the Subscription's request and no messages will be passed. + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::QoS(1).transient_local(); + + auto publisher = node_->create_generic_publisher(topic_name, topic_type, qos); + auto subscription = node_->create_subscription( + topic_name, qos, + [](std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); +} diff --git a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp new file mode 100644 index 0000000000..8cdcfc19c0 --- /dev/null +++ b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp @@ -0,0 +1,77 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rcpputils/shared_library.hpp" + +#include "rclcpp/typesupport_helpers.hpp" + +using namespace ::testing; // NOLINT + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_no_type) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("just_a_package_name", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_slash_at_the_start_only) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("/name_with_slash_at_start", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_slash_at_the_end_only) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("name_with_slash_at_end/", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_library_cannot_be_found) { + EXPECT_THROW( + rclcpp::get_typesupport_library("invalid/message", "rosidl_typesupport_cpp"), + std::runtime_error); +} + +TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_legacy_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/BasicTypes", "rosidl_typesupport_cpp"); + auto string_typesupport = rclcpp::get_typesupport_handle( + "test_msgs/BasicTypes", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(string_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::exception & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp"); + auto string_typesupport = rclcpp::get_typesupport_handle( + "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(string_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 451c09e383..d81750ba08 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -57,6 +57,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -283,6 +285,35 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. + /** + * \sa rclcpp::Node::create_generic_publisher + */ + template> + std::shared_ptr create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) + ); + + /// Create and return a GenericSubscription. + /** + * \sa rclcpp::Node::create_generic_subscription + */ + template> + std::shared_ptr create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) + ); + /// Declare and initialize a parameter, return the effective value. /** * \sa rclcpp::Node::declare_parameter diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 5582d8aca6..77598fdcb7 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -22,12 +22,14 @@ #include #include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/event.hpp" -#include "rclcpp/experimental/intra_process_manager.hpp" -#include "rclcpp/parameter.hpp" +#include "rclcpp/create_generic_publisher.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" @@ -130,6 +132,46 @@ LifecycleNode::create_service( service_name, std::forward(callback), qos_profile, group); } +template +std::shared_ptr +LifecycleNode::create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_generic_publisher( + node_topics_, + // TODO(karsten1987): LifecycleNode is currently not supporting subnamespaces + // see https://github.com/ros2/rclcpp/issues/1614 + topic_name, + topic_type, + qos, + options + ); +} + +template +std::shared_ptr +LifecycleNode::create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) +{ + return rclcpp::create_generic_subscription( + node_topics_, + // TODO(karsten1987): LifecycleNode is currently not supporting subnamespaces + // see https://github.com/ros2/rclcpp/issues/1614 + topic_name, + topic_type, + qos, + std::move(callback), + options + ); +} + template auto LifecycleNode::declare_parameter(