diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 66b0f10bb3..f2ee5e63fe 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -65,6 +65,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp src/rclcpp/publisher_base.cpp + src/rclcpp/qos.cpp src/rclcpp/qos_event.cpp src/rclcpp/service.cpp src/rclcpp/signal_handler.cpp @@ -142,6 +143,8 @@ if(BUILD_TESTING) find_package(rmw_implementation_cmake REQUIRED) + include(cmake/rclcpp_add_build_failure_test.cmake) + ament_add_gtest(test_client test/test_client.cpp) if(TARGET test_client) ament_target_dependencies(test_client @@ -201,6 +204,34 @@ if(BUILD_TESTING) ) target_link_libraries(test_node ${PROJECT_NAME}) endif() + + ament_add_gtest(test_node_interfaces__get_node_interfaces + test/node_interfaces/test_get_node_interfaces.cpp) + if(TARGET test_node_interfaces__get_node_interfaces) + target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) + endif() + + # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node + # ${PROJECT_NAME}) + + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node + # ${PROJECT_NAME}) + + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node + # ${PROJECT_NAME}) + + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node + # ${PROJECT_NAME}) + ament_add_gtest(test_node_global_args test/test_node_global_args.cpp) if(TARGET test_node_global_args) ament_target_dependencies(test_node_global_args @@ -245,15 +276,6 @@ if(BUILD_TESTING) ) target_link_libraries(test_publisher ${PROJECT_NAME}) endif() - ament_add_gtest(test_pub_sub_option_interface test/test_pub_sub_option_interface.cpp) - if(TARGET test_pub_sub_option_interface) - ament_target_dependencies(test_pub_sub_option_interface - test_msgs - ) - target_link_libraries(test_pub_sub_option_interface - ${PROJECT_NAME} - ) - endif() ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) ament_target_dependencies(test_publisher_subscription_count_api diff --git a/rclcpp/cmake/rclcpp_add_build_failure_test.cmake b/rclcpp/cmake/rclcpp_add_build_failure_test.cmake new file mode 100644 index 0000000000..5ccde6eba8 --- /dev/null +++ b/rclcpp/cmake/rclcpp_add_build_failure_test.cmake @@ -0,0 +1,56 @@ +# Copyright 2019 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. + +# +# Register a test which tries to compile a file and expects it to fail to build. +# +# This will create two targets, one by the given target name and a test target +# which has the same name prefixed with `test_`. +# For example, if target is `should_not_compile__use_const_argument` then there +# will be an executable target called `should_not_compile__use_const_argument` +# and a test target called `test_should_not_compile__use_const_argument`. +# +# :param target: the name of the target to be created +# :type target: string +# :param ARGN: the list of source files to be used to create the test executable +# :type ARGN: list of strings +# +macro(rclcpp_add_build_failure_test target) + if(${ARGC} EQUAL 0) + message( + FATAL_ERROR + "rclcpp_add_build_failure_test() requires a target name and " + "at least one source file") + endif() + + add_executable(${target} ${ARGN}) + set_target_properties(${target} + PROPERTIES + EXCLUDE_FROM_ALL TRUE + EXCLUDE_FROM_DEFAULT_BUILD TRUE) + + add_test( + NAME test_${target} + COMMAND + ${CMAKE_COMMAND} + --build . + --target ${target} + --config $ + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) + set_tests_properties(test_${target} + PROPERTIES + WILL_FAIL TRUE + LABELS "build_failure" + ) +endmacro() diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 1e60f7faf6..d117376517 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -65,7 +65,8 @@ void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_al // Convert a std::allocator_traits-formatted Allocator into an rcl allocator template< - typename T, typename Alloc, + typename T, + typename Alloc, typename std::enable_if>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { @@ -83,7 +84,8 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) // TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator template< - typename T, typename Alloc, + typename T, + typename Alloc, typename std::enable_if>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index e21b00687b..29b11f2185 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -18,14 +18,23 @@ #include #include +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/publisher_factory.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rmw/qos_profiles.h" namespace rclcpp { -template +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = ::rclcpp::Publisher> +// cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function +[[deprecated("use alternative rclcpp::create_publisher() signatures")]] std::shared_ptr create_publisher( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, @@ -50,6 +59,63 @@ create_publisher( return std::dynamic_pointer_cast(pub); } +/// Create and return a publisher of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface. + */ +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = ::rclcpp::Publisher, + typename NodeT> +std::shared_ptr +create_publisher( + NodeT & node, + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() +)) +{ + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics = get_node_topics_interface(node); + + std::shared_ptr allocator = options.allocator; + if (!allocator) { + allocator = std::make_shared(); + } + + bool use_intra_process; + switch (options.use_intra_process_comm) { + case IntraProcessSetting::Enable: + use_intra_process = true; + break; + case IntraProcessSetting::Disable: + use_intra_process = false; + break; + case IntraProcessSetting::NodeDefault: + use_intra_process = node_topics->get_node_base_interface()->get_use_intra_process_default(); + break; + default: + throw std::runtime_error("Unrecognized IntraProcessSetting value"); + break; + } + + // TODO(wjwwood): convert all of the interfaces to use QoS and PublisherOptionsBase + auto pub = node_topics->create_publisher( + topic_name, + rclcpp::create_publisher_factory( + options.event_callbacks, + allocator + ), + options.template to_rcl_publisher_options(qos), + use_intra_process + ); + node_topics->add_publisher(pub, options.callback_group); + return std::dynamic_pointer_cast(pub); +} + } // namespace rclcpp #endif // RCLCPP__CREATE_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index e765d376e2..fa11affc7b 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -19,8 +19,11 @@ #include #include +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/subscription_factory.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/qos.hpp" #include "rmw/qos_profiles.h" namespace rclcpp @@ -32,6 +35,8 @@ template< typename AllocatorT, typename CallbackMessageT, typename SubscriptionT = rclcpp::Subscription> +// cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function +[[deprecated("use alternative rclcpp::create_subscription() signatures")]] typename std::shared_ptr create_subscription( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, @@ -67,6 +72,75 @@ create_subscription( return std::dynamic_pointer_cast(sub); } +/// Create and return a subscription of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename NodeT> +typename std::shared_ptr +create_subscription( + NodeT && node, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, AllocatorT>::SharedPtr + msg_mem_strat = nullptr) +{ + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics = get_node_topics_interface(std::forward(node)); + + if (!msg_mem_strat) { + using rclcpp::message_memory_strategy::MessageMemoryStrategy; + msg_mem_strat = MessageMemoryStrategy::create_default(); + } + + std::shared_ptr allocator = options.allocator; + if (!allocator) { + allocator = std::make_shared(); + } + auto factory = rclcpp::create_subscription_factory + ( + std::forward(callback), options.event_callbacks, msg_mem_strat, allocator); + + bool use_intra_process; + switch (options.use_intra_process_comm) { + case IntraProcessSetting::Enable: + use_intra_process = true; + break; + case IntraProcessSetting::Disable: + use_intra_process = false; + break; + case IntraProcessSetting::NodeDefault: + use_intra_process = node_topics->get_node_base_interface()->get_use_intra_process_default(); + break; + default: + throw std::runtime_error("Unrecognized IntraProcessSetting value"); + break; + } + + // TODO(wjwwood): convert all of the interfaces to use QoS and SubscriptionOptionsBase + auto sub = node_topics->create_subscription( + topic_name, + factory, + options.template to_rcl_subscription_options(qos), + use_intra_process); + node_topics->add_subscription(sub, options.callback_group); + return std::dynamic_pointer_cast(sub); +} + } // namespace rclcpp #endif // RCLCPP__CREATE_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index b92ea42aec..7a80cb37d4 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -23,91 +23,87 @@ namespace rclcpp { -class Duration +class RCLCPP_PUBLIC Duration { public: - RCLCPP_PUBLIC Duration(int32_t seconds, uint32_t nanoseconds); - RCLCPP_PUBLIC - explicit Duration( - rcl_duration_value_t nanoseconds); + explicit Duration(rcl_duration_value_t nanoseconds); - RCLCPP_PUBLIC - explicit Duration( - std::chrono::nanoseconds nanoseconds); + explicit Duration(std::chrono::nanoseconds nanoseconds); - RCLCPP_PUBLIC - Duration( - const builtin_interfaces::msg::Duration & duration_msg); + // intentionally not using explicit to create a conversion constructor + template + // cppcheck-suppress noExplicitConstructor + Duration(const std::chrono::duration & duration) // NOLINT(runtime/explicit) + : Duration(std::chrono::duration_cast(duration)) + {} + + // cppcheck-suppress noExplicitConstructor + Duration(const builtin_interfaces::msg::Duration & duration_msg); // NOLINT(runtime/explicit) - RCLCPP_PUBLIC explicit Duration(const rcl_duration_t & duration); - RCLCPP_PUBLIC Duration(const Duration & rhs); - RCLCPP_PUBLIC - virtual ~Duration(); + virtual ~Duration() = default; - RCLCPP_PUBLIC operator builtin_interfaces::msg::Duration() const; - RCLCPP_PUBLIC + // cppcheck-suppress operatorEq // this is a false positive from cppcheck Duration & operator=(const Duration & rhs); - RCLCPP_PUBLIC Duration & operator=(const builtin_interfaces::msg::Duration & Duration_msg); - RCLCPP_PUBLIC bool operator==(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator<(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator<=(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator>=(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator>(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC Duration operator+(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC Duration operator-(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC - static Duration + static + Duration max(); - RCLCPP_PUBLIC Duration operator*(double scale) const; - RCLCPP_PUBLIC rcl_duration_value_t nanoseconds() const; /// \return the duration in seconds as a floating point number. /// \warning Depending on sizeof(double) there could be significant precision loss. /// When an exact time is required use nanoseconds() instead. - RCLCPP_PUBLIC double seconds() const; + template + DurationT + to_chrono() const + { + return std::chrono::duration_cast(std::chrono::nanoseconds(this->nanoseconds())); + } + + rmw_time_t + to_rmw_time() const; + private: rcl_duration_t rcl_duration_; }; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c987071592..8c622509e2 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -42,7 +42,6 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" -#include "rclcpp/node_options.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" @@ -53,9 +52,11 @@ #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_options.hpp" @@ -144,8 +145,27 @@ class Node : public std::enable_shared_from_this /// Create and return a Publisher. /** + * The rclcpp::QoS has several convenient constructors, including a + * conversion constructor for size_t, which mimics older API's that + * allows just a string and size_t to create a publisher. + * + * For example, all of these cases will work: + * + * pub = node->create_publisher("chatter", 10); // implicitly KeepLast + * pub = node->create_publisher("chatter", QoS(10)); // implicitly KeepLast + * pub = node->create_publisher("chatter", QoS(KeepLast(10))); + * pub = node->create_publisher("chatter", QoS(KeepAll())); + * pub = node->create_publisher("chatter", QoS(1).best_effort().volatile()); + * { + * rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data); + * pub = node->create_publisher("chatter", custom_qos); + * } + * + * The publisher options may optionally be passed as the third argument for + * any of the above cases. + * * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_history_depth The depth of the publisher message queue. + * \param[in] qos The Quality of Service settings for the publisher. * \param[in] options Additional options for the created Publisher. * \return Shared pointer to the created publisher. */ @@ -156,30 +176,29 @@ class Node : public std::enable_shared_from_this std::shared_ptr create_publisher( const std::string & topic_name, - size_t qos_history_depth, - const PublisherOptionsWithAllocator & - options = PublisherOptionsWithAllocator()); + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options = + PublisherOptionsWithAllocator() + ); /// Create and return a Publisher. /** * \param[in] topic_name The topic for this publisher to publish on. * \param[in] qos_history_depth The depth of the publisher message queue. - * \param[in] allocator Optional custom allocator. + * \param[in] allocator Custom allocator. * \return Shared pointer to the created publisher. */ template< - typename MessageT, typename Alloc = std::allocator, - typename PublisherT = ::rclcpp::Publisher> + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = ::rclcpp::Publisher> // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function - [[deprecated( - "use the create_publisher(const std::string &, size_t, const PublisherOptions & = " - "PublisherOptions()) signature instead")]] + [[deprecated("use create_publisher(const std::string &, const rclcpp::QoS &, ...) instead")]] std::shared_ptr create_publisher( const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator, - IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault); + std::shared_ptr allocator); /// Create and return a Publisher. /** @@ -189,14 +208,16 @@ class Node : public std::enable_shared_from_this * \return Shared pointer to the created publisher. */ template< - typename MessageT, typename Alloc = std::allocator, - typename PublisherT = ::rclcpp::Publisher> + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = ::rclcpp::Publisher> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function + [[deprecated("use create_publisher(const std::string &, const rclcpp::QoS &, ...) instead")]] std::shared_ptr create_publisher( const std::string & topic_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - std::shared_ptr allocator = nullptr, - IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault); + std::shared_ptr allocator = nullptr); /// Create and return a Subscription. /** @@ -220,10 +241,10 @@ class Node : public std::enable_shared_from_this std::shared_ptr create_subscription( const std::string & topic_name, + const rclcpp::QoS & qos, CallbackT && callback, - size_t qos_history_depth, - const SubscriptionOptionsWithAllocator & - options = SubscriptionOptionsWithAllocator(), + const SubscriptionOptionsWithAllocator & options = + SubscriptionOptionsWithAllocator(), typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, AllocatorT >::SharedPtr @@ -250,6 +271,10 @@ class Node : public std::enable_shared_from_this typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription< typename rclcpp::subscription_traits::has_message_type::type, Alloc>> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function + [[deprecated( + "use create_subscription(const std::string &, const rclcpp::QoS &, CallbackT, ...) instead" + )]] std::shared_ptr create_subscription( const std::string & topic_name, @@ -260,8 +285,7 @@ class Node : public std::enable_shared_from_this typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr, - IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault); + std::shared_ptr allocator = nullptr); /// Create and return a Subscription. /** @@ -284,21 +308,21 @@ class Node : public std::enable_shared_from_this typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription< typename rclcpp::subscription_traits::has_message_type::type, Alloc>> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function [[deprecated( - "use the create_subscription(const std::string &, CallbackT &&, size_t, " - "const SubscriptionOptions & = SubscriptionOptions(), ...) signature instead")]] + "use create_subscription(const std::string &, const rclcpp::QoS &, CallbackT, ...) instead" + )]] std::shared_ptr create_subscription( const std::string & topic_name, CallbackT && callback, size_t qos_history_depth, - rclcpp::callback_group::CallbackGroup::SharedPtr group, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, bool ignore_local_publications = false, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr, - IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault); + std::shared_ptr allocator = nullptr); /// Create a timer. /** @@ -1118,12 +1142,12 @@ class Node : public std::enable_shared_from_this * with a leading '/'. */ RCLCPP_PUBLIC - Node::SharedPtr + rclcpp::Node::SharedPtr create_sub_node(const std::string & sub_namespace); /// Return the NodeOptions used when creating this node. RCLCPP_PUBLIC - const NodeOptions & + const rclcpp::NodeOptions & get_node_options() const; /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). @@ -1170,7 +1194,7 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; - const NodeOptions node_options_; + const rclcpp::NodeOptions node_options_; const std::string sub_namespace_; const std::string effective_namespace_; }; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index a3eb298ccc..94352fabe7 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -36,11 +36,12 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/intra_process_manager.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -67,68 +68,42 @@ template std::shared_ptr Node::create_publisher( const std::string & topic_name, - size_t qos_history_depth, + const rclcpp::QoS & qos, const PublisherOptionsWithAllocator & options) { - std::shared_ptr allocator = options.allocator; - if (!allocator) { - allocator = std::make_shared(); - } - rmw_qos_profile_t qos_profile = options.qos_profile; - qos_profile.depth = qos_history_depth; - - bool use_intra_process; - switch (options.use_intra_process_comm) { - case IntraProcessSetting::Enable: - use_intra_process = true; - break; - case IntraProcessSetting::Disable: - use_intra_process = false; - break; - case IntraProcessSetting::NodeDefault: - use_intra_process = this->get_node_options().use_intra_process_comms(); - break; - default: - throw std::runtime_error("Unrecognized IntraProcessSetting value"); - break; - } - return rclcpp::create_publisher( - this->node_topics_.get(), + *this, extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - qos_profile, - options.event_callbacks, - options.callback_group, - use_intra_process, - allocator); + qos, + options); } -template +template std::shared_ptr Node::create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator, - IntraProcessSetting use_intra_process_comm) + const std::string & topic_name, + size_t qos_history_depth, + std::shared_ptr allocator) { - PublisherOptionsWithAllocator pub_options; + PublisherOptionsWithAllocator pub_options; pub_options.allocator = allocator; - pub_options.use_intra_process_comm = use_intra_process_comm; - return this->create_publisher( - topic_name, qos_history_depth, pub_options); + return this->create_publisher( + topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), pub_options); } -template +template std::shared_ptr Node::create_publisher( - const std::string & topic_name, const rmw_qos_profile_t & qos_profile, - std::shared_ptr allocator, IntraProcessSetting use_intra_process_comm) + const std::string & topic_name, + const rmw_qos_profile_t & qos_profile, + std::shared_ptr allocator) { - PublisherOptionsWithAllocator pub_options; - pub_options.qos_profile = qos_profile; + rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile)); + qos.get_rmw_qos_profile() = qos_profile; + + PublisherOptionsWithAllocator pub_options; pub_options.allocator = allocator; - pub_options.use_intra_process_comm = use_intra_process_comm; - return this->create_publisher( - topic_name, qos_profile.depth, pub_options); + return this->create_publisher(topic_name, qos, pub_options); } template< @@ -139,56 +114,20 @@ template< std::shared_ptr Node::create_subscription( const std::string & topic_name, + const rclcpp::QoS & qos, CallbackT && callback, - size_t qos_history_depth, const SubscriptionOptionsWithAllocator & options, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, AllocatorT>::SharedPtr msg_mem_strat) { - using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type::type; - - std::shared_ptr allocator = options.allocator; - if (!allocator) { - allocator = std::make_shared(); - } - - rmw_qos_profile_t qos_profile = options.qos_profile; - qos_profile.depth = qos_history_depth; - - if (!msg_mem_strat) { - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - msg_mem_strat = MessageMemoryStrategy::create_default(); - } - - bool use_intra_process; - switch (options.use_intra_process_comm) { - case IntraProcessSetting::Enable: - use_intra_process = true; - break; - case IntraProcessSetting::Disable: - use_intra_process = false; - break; - case IntraProcessSetting::NodeDefault: - use_intra_process = this->get_node_options().use_intra_process_comms(); - break; - default: - throw std::runtime_error("Unrecognized IntraProcessSetting value"); - break; - } - - return rclcpp::create_subscription< - MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>( - this->node_topics_.get(), + return rclcpp::create_subscription( + *this, extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, std::forward(callback), - qos_profile, - options.event_callbacks, - options.callback_group, - options.ignore_local_publications, - use_intra_process, - msg_mem_strat, - allocator); + options, + msg_mem_strat); } template< @@ -206,18 +145,18 @@ Node::create_subscription( typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr msg_mem_strat, - std::shared_ptr allocator, - IntraProcessSetting use_intra_process_comm) + std::shared_ptr allocator) { + rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile)); + qos.get_rmw_qos_profile() = qos_profile; + SubscriptionOptionsWithAllocator sub_options; - sub_options.qos_profile = qos_profile; sub_options.callback_group = group; sub_options.ignore_local_publications = ignore_local_publications; sub_options.allocator = allocator; - sub_options.use_intra_process_comm = use_intra_process_comm; return this->create_subscription( - topic_name, std::forward(callback), qos_profile.depth, sub_options, msg_mem_strat); + topic_name, qos, std::forward(callback), sub_options, msg_mem_strat); } template< @@ -235,17 +174,19 @@ Node::create_subscription( typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr msg_mem_strat, - std::shared_ptr allocator, - IntraProcessSetting use_intra_process_comm) + std::shared_ptr allocator) { SubscriptionOptionsWithAllocator sub_options; sub_options.callback_group = group; sub_options.ignore_local_publications = ignore_local_publications; sub_options.allocator = allocator; - sub_options.use_intra_process_comm = use_intra_process_comm; return this->create_subscription( - topic_name, std::forward(callback), qos_history_depth, sub_options, msg_mem_strat); + topic_name, + rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), + std::forward(callback), + sub_options, + msg_mem_strat); } template diff --git a/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp new file mode 100644 index 0000000000..2c7ccd543b --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp @@ -0,0 +1,147 @@ +// Copyright 2019 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 RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_topics_interface.hpp" + +/// This header provides the get_node_topics_interface() template function. +/** + * This function is useful for getting the NodeTopicsInterface pointer from + * various kinds of Node-like classes. + * + * It's able to get the NodeTopicsInterface pointer so long as the class + * has a method called ``get_node_topics_interface()`` which returns + * either a pointer (const or not) to a NodeTopicsInterface or a + * std::shared_ptr to a NodeTopicsInterface. + */ + +namespace rclcpp +{ +namespace node_interfaces +{ + +namespace detail +{ + +// This is a meta-programming checker for if a given Node-like object has a +// getter called get_node_topics_interface() which returns various types, +// e.g. const pointer or a shared pointer. +template +struct has_get_node_topics_interface +{ +private: + template + static constexpr + auto + check(T *)->typename std::is_same< + decltype(std::declval().get_node_topics_interface()), + ReturnType + >::type; + + template + static constexpr + std::false_type + check(...); + +public: + using type = decltype(check(nullptr)); + static constexpr bool value = type::value; +}; + +// If NodeType is a pointer to NodeTopicsInterface already (just normal function overload). +inline +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterface * pointer) +{ + return pointer; +} + +// If NodeType has a method called get_node_topics_interface() which returns a shared pointer. +template< + typename NodeType, + typename std::enable_if::type, + std::shared_ptr + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_topics_interface().get(); +} + +// If NodeType has a method called get_node_topics_interface() which returns a pointer. +template< + typename NodeType, + typename std::enable_if::type, + rclcpp::node_interfaces::NodeTopicsInterface * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_topics_interface(); +} + +// Forward shared_ptr's to const node pointer signatures. +template< + typename NodeType, + typename std::enable_if::type::element_type> * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(NodeType node_shared_pointer) +{ + return get_node_topics_interface_from_pointer(node_shared_pointer->get()); +} + +} // namespace detail + +/// Get the NodeTopicsInterface as a pointer from a pointer to a "Node like" object. +template< + typename NodeType, + typename std::enable_if::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface(NodeType && node_pointer) +{ + // Forward pointers to detail implmentation directly. + return detail::get_node_topics_interface_from_pointer(std::forward(node_pointer)); +} + +/// Get the NodeTopicsInterface as a pointer from a "Node like" object. +template< + typename NodeType, + typename std::enable_if::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface(NodeType && node_reference) +{ + // Forward references to detail implmentation as a pointer. + return detail::get_node_topics_interface_from_pointer(&node_reference); +} + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 77379c2858..5bede31fe1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -19,10 +19,11 @@ #include #include +#include "rcl/node.h" #include "rclcpp/context.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node_options.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -34,13 +35,15 @@ namespace node_interfaces class NodeBase : public NodeBaseInterface { public: - RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase) RCLCPP_PUBLIC NodeBase( const std::string & node_name, const std::string & namespace_, - const NodeOptions & options); + rclcpp::Context::SharedPtr context, + const rcl_node_options_t & rcl_node_options, + bool use_intra_process_default); RCLCPP_PUBLIC virtual @@ -126,10 +129,16 @@ class NodeBase : public NodeBaseInterface std::unique_lock acquire_notify_guard_condition_lock() const; + RCLCPP_PUBLIC + virtual + bool + get_use_intra_process_default() const; + private: RCLCPP_DISABLE_COPY(NodeBase) rclcpp::Context::SharedPtr context_; + bool use_intra_process_default_; std::shared_ptr node_handle_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index a2228fb617..d0247ee0f2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -155,6 +155,12 @@ class NodeBaseInterface virtual std::unique_lock acquire_notify_guard_condition_lock() const = 0; + + /// Return the default preference for using intra process communication. + RCLCPP_PUBLIC + virtual + bool + get_use_intra_process_default() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 64969e87e0..f9d5d445f7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -64,10 +64,10 @@ class NodeParameters : public NodeParametersInterface const node_interfaces::NodeServicesInterface::SharedPtr node_services, const node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::vector & initial_parameters, - bool use_intra_process, bool start_parameter_services, bool start_parameter_event_publisher, - const rmw_qos_profile_t & parameter_event_qos_profile, + const rclcpp::QoS & parameter_event_qos, + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, bool allow_undeclared_parameters, bool automatically_declare_initial_parameters); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 40beb3aacb..a36c8e6bc4 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -42,45 +42,44 @@ class NodeTopics : public NodeTopicsInterface explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base); RCLCPP_PUBLIC - virtual - ~NodeTopics(); + ~NodeTopics() override; RCLCPP_PUBLIC - virtual rclcpp::PublisherBase::SharedPtr create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, - rcl_publisher_options_t & publisher_options, - bool use_intra_process); + const rcl_publisher_options_t & publisher_options, + bool use_intra_process) override; RCLCPP_PUBLIC - virtual void add_publisher( rclcpp::PublisherBase::SharedPtr publisher, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC - virtual rclcpp::SubscriptionBase::SharedPtr create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, - rcl_subscription_options_t & subscription_options, - bool use_intra_process); + const rcl_subscription_options_t & subscription_options, + bool use_intra_process) override; RCLCPP_PUBLIC - virtual void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface * + get_node_base_interface() const override; private: RCLCPP_DISABLE_COPY(NodeTopics) - NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeBaseInterface * node_base_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index d888e3ccd2..7e70dc067f 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -50,7 +50,7 @@ class NodeTopicsInterface create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, - rcl_publisher_options_t & publisher_options, + const rcl_publisher_options_t & publisher_options, bool use_intra_process) = 0; RCLCPP_PUBLIC @@ -66,7 +66,7 @@ class NodeTopicsInterface create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, - rcl_subscription_options_t & subscription_options, + const rcl_subscription_options_t & subscription_options, bool use_intra_process) = 0; RCLCPP_PUBLIC @@ -75,6 +75,11 @@ class NodeTopicsInterface add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::node_interfaces::NodeBaseInterface * + get_node_base_interface() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index f3dfd6987e..bab6bccbf8 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -23,6 +23,8 @@ #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -43,7 +45,9 @@ class NodeOptions * - use_intra_process_comms = false * - start_parameter_services = true * - start_parameter_event_publisher = true - * - parameter_event_qos_profile = rmw_qos_profile_parameter_events + * - parameter_event_qos = rclcpp::ParameterEventQoS + * - with history setting and depth from rmw_qos_profile_parameter_events + * - parameter_event_publisher_options = rclcpp::PublisherOptionsBase * - allow_undeclared_parameters = false * - automatically_declare_initial_parameters = false * - allocator = rcl_get_default_allocator() @@ -202,18 +206,36 @@ class NodeOptions NodeOptions & start_parameter_event_publisher(bool start_parameter_event_publisher); - /// Return a reference to the parameter_event_qos_profile QoS. + /// Return a reference to the parameter_event_qos QoS. RCLCPP_PUBLIC - const rmw_qos_profile_t & - parameter_event_qos_profile() const; + const rclcpp::QoS & + parameter_event_qos() const; - /// Set the parameter_event_qos_profile QoS, return this for parameter idiom. + /// Set the parameter_event_qos QoS, return this for parameter idiom. /** * The QoS settings to be used for the parameter event publisher, if enabled. */ RCLCPP_PUBLIC NodeOptions & - parameter_event_qos_profile(const rmw_qos_profile_t & parameter_event_qos_profile); + parameter_event_qos(const rclcpp::QoS & parameter_event_qos); + + /// Return a reference to the parameter_event_publisher_options. + RCLCPP_PUBLIC + const rclcpp::PublisherOptionsBase & + parameter_event_publisher_options() const; + + /// Set the parameter_event_publisher_options, return this for parameter idiom. + /** + * The QoS settings to be used for the parameter event publisher, if enabled. + * + * \todo(wjwwood): make this take/store an instance of + * rclcpp::PublisherOptionsWithAllocator, but to do that requires + * NodeOptions to also be templated based on the Allocator type. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_event_publisher_options( + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options); /// Return the allow_undeclared_parameters flag. RCLCPP_PUBLIC @@ -290,7 +312,11 @@ class NodeOptions bool start_parameter_event_publisher_ {true}; - rmw_qos_profile_t parameter_event_qos_profile_ {rmw_qos_profile_parameter_events}; + rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( + rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) + ); + + rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase(); bool allow_undeclared_parameters_ {false}; diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 6b68a793fa..8af9429cfc 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -110,29 +110,23 @@ class AsyncParametersClient template< typename CallbackT, - typename Alloc = std::allocator, - typename SubscriptionT = - rclcpp::Subscription> + typename AllocatorT = std::allocator> typename rclcpp::Subscription::SharedPtr - on_parameter_event(CallbackT && callback) + on_parameter_event( + CallbackT && callback, + const rclcpp::QoS & qos = ( + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)) + ), + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + )) { - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - auto msg_mem_strat = - MessageMemoryStrategy::create_default(); - - using rcl_interfaces::msg::ParameterEvent; - return rclcpp::create_subscription< - ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>( - this->node_topics_interface_.get(), + return rclcpp::create_subscription( + this->node_topics_interface_, "parameter_events", + qos, std::forward(callback), - rmw_qos_profile_default, - SubscriptionEventCallbacks(), - nullptr, // group, - false, // ignore_local_publications, - false, // use_intra_process_comms_, - msg_mem_strat, - std::make_shared()); + options); } RCLCPP_PUBLIC @@ -155,7 +149,7 @@ class AsyncParametersClient wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); private: - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; rclcpp::Client::SharedPtr get_parameters_client_; rclcpp::Client::SharedPtr get_parameter_types_client_; diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index ce2196a15f..8e6e8e27fb 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -49,7 +49,7 @@ struct PublisherFactory rclcpp::PublisherBase::SharedPtr( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, - rcl_publisher_options_t & publisher_options)>; + const rcl_publisher_options_t & publisher_options)>; PublisherFactoryFunction create_typed_publisher; }; @@ -68,15 +68,17 @@ create_publisher_factory( [event_callbacks, allocator]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, - rcl_publisher_options_t & publisher_options) -> std::shared_ptr + const rcl_publisher_options_t & publisher_options + ) -> std::shared_ptr { + auto options_copy = publisher_options; auto message_alloc = std::make_shared(*allocator.get()); - publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); + options_copy.allocator = allocator::get_rcl_allocator(*message_alloc.get()); return std::make_shared( node_base, topic_name, - publisher_options, + options_copy, event_callbacks, message_alloc); }; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index acdec02718..d1a7f6455c 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -21,24 +21,54 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl/publisher.h" namespace rclcpp { -/// Structure containing optional configuration for Publishers. -template -struct PublisherOptionsWithAllocator +/// Non-templated part of PublisherOptionsWithAllocator. +struct PublisherOptionsBase { + /// Setting to explicitly set intraprocess communications. + IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + + /// Callbacks for various events related to publishers. PublisherEventCallbacks event_callbacks; - /// The quality of service profile to pass on to the rmw implementation. - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + + /// Callback group in which the waitable items from the publisher should be placed. rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; +}; + +/// Structure containing optional configuration for Publishers. +template +struct PublisherOptionsWithAllocator : public PublisherOptionsBase +{ /// Optional custom allocator. - std::shared_ptr allocator; - /// Setting to explicitly set intraprocess communications. - IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + std::shared_ptr allocator = nullptr; + + PublisherOptionsWithAllocator() {} + + /// Constructor using base class as input. + explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base) + : PublisherOptionsBase(publisher_options_base) + {} + + /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t. + template + rcl_publisher_options_t + to_rcl_publisher_options(const rclcpp::QoS & qos) const + { + rcl_publisher_options_t result; + using AllocatorTraits = std::allocator_traits; + using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; + auto message_alloc = std::make_shared(*allocator.get()); + result.allocator = allocator::get_rcl_allocator(*message_alloc); + result.qos = qos.get_rmw_qos_profile(); + return result; + } }; using PublisherOptions = PublisherOptionsWithAllocator>; diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp new file mode 100644 index 0000000000..0943d9ad96 --- /dev/null +++ b/rclcpp/include/rclcpp/qos.hpp @@ -0,0 +1,203 @@ +// Copyright 2019 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 RCLCPP__QOS_HPP_ +#define RCLCPP__QOS_HPP_ + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. +struct RCLCPP_PUBLIC QoSInitialization +{ + rmw_qos_history_policy_t history_policy; + size_t depth; + + /// Constructor which takes both a history policy and a depth (even if it would be unused). + QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg); + + /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth. + static + QoSInitialization + from_rmw(const rmw_qos_profile_t & rmw_qos); +}; + +/// Use to initialize the QoS with the keep_all history setting. +struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization +{ + KeepAll(); +}; + +/// Use to initialize the QoS with the keep_last history setting and the given depth. +struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization +{ + explicit KeepLast(size_t depth); +}; + +/// Encapsulation of Quality of Service settings. +class RCLCPP_PUBLIC QoS +{ +public: + /// Constructor which allows you to construct a QoS by giving the only required settings. + explicit + QoS( + const QoSInitialization & qos_initialization, + const rmw_qos_profile_t & initial_profile = rmw_qos_profile_default); + + /// Conversion constructor to ease construction in the common case of just specifying depth. + /** + * Convenience constructor, equivalent to QoS(KeepLast(history_depth)). + */ + // cppcheck-suppress noExplicitConstructor + QoS(size_t history_depth); // NOLINT(runtime/explicit): conversion constructor + + /// Return the rmw qos profile. + rmw_qos_profile_t & + get_rmw_qos_profile(); + + /// Return the rmw qos profile. + const rmw_qos_profile_t & + get_rmw_qos_profile() const; + + /// Set the history policy. + QoS & + history(rmw_qos_history_policy_t history); + + /// Set the history to keep last. + QoS & + keep_last(size_t depth); + + /// Set the history to keep all. + QoS & + keep_all(); + + /// Set the reliability setting. + QoS & + reliability(rmw_qos_reliability_policy_t reliability); + + /// Set the reliability setting to reliable. + QoS & + reliable(); + + /// Set the reliability setting to best effort. + QoS & + best_effort(); + + /// Set the durability setting. + QoS & + durability(rmw_qos_durability_policy_t durability); + + /// Set the durability setting to volatile. + QoS & + volitile(); + + /// Set the durability setting to transient local. + QoS & + transient_local(); + + /// Set the deadline setting. + QoS & + deadline(rmw_time_t deadline); + + /// Set the deadline setting, rclcpp::Duration. + QoS & + deadline(const rclcpp::Duration & deadline); + + /// Set the lifespan setting. + QoS & + lifespan(rmw_time_t lifespan); + + /// Set the lifespan setting, rclcpp::Duration. + QoS & + lifespan(const rclcpp::Duration & lifespan); + + /// Set the liveliness setting. + QoS & + liveliness(rmw_qos_liveliness_policy_t liveliness); + + /// Set the liveliness_lease_duration setting. + QoS & + liveliness_lease_duration(rmw_time_t liveliness_lease_duration); + + /// Set the liveliness_lease_duration setting, rclcpp::Duration. + QoS & + liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration); + + /// Set the avoid_ros_namespace_conventions setting. + QoS & + avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions); + +private: + rmw_qos_profile_t rmw_qos_profile_; +}; + +class RCLCPP_PUBLIC SensorDataQoS : public QoS +{ +public: + explicit + SensorDataQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_sensor_data) + )); +}; + +class RCLCPP_PUBLIC ParametersQoS : public QoS +{ +public: + explicit + ParametersQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_parameters) + )); +}; + +class RCLCPP_PUBLIC ServicesQoS : public QoS +{ +public: + explicit + ServicesQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_services_default) + )); +}; + +class RCLCPP_PUBLIC ParameterEventsQoS : public QoS +{ +public: + explicit + ParameterEventsQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) + )); +}; + +class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS +{ +public: + explicit + SystemDefaultsQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_system_default) + )); +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 7fe6bdef3b..9b46fe33d2 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -51,7 +51,7 @@ struct SubscriptionFactory rclcpp::SubscriptionBase::SharedPtr( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, - rcl_subscription_options_t & subscription_options)>; + const rcl_subscription_options_t & subscription_options)>; SubscriptionFactoryFunction create_typed_subscription; @@ -95,10 +95,11 @@ create_subscription_factory( [allocator, msg_mem_strat, any_subscription_callback, event_callbacks, message_alloc]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, - rcl_subscription_options_t & subscription_options + const rcl_subscription_options_t & subscription_options ) -> rclcpp::SubscriptionBase::SharedPtr { - subscription_options.allocator = + auto options_copy = subscription_options; + options_copy.allocator = rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); using rclcpp::Subscription; @@ -108,7 +109,7 @@ create_subscription_factory( node_base->get_shared_rcl_node_handle(), *rosidl_typesupport_cpp::get_message_type_support_handle(), topic_name, - subscription_options, + options_copy, any_subscription_callback, event_callbacks, msg_mem_strat); diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index a86612e25b..d76be04ee9 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -21,29 +21,57 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { -/// Structure containing optional configuration for Subscriptions. -template -struct SubscriptionOptionsWithAllocator +/// Non-template base class for subscription options. +struct SubscriptionOptionsBase { + /// Callbacks for events related to this subscription. SubscriptionEventCallbacks event_callbacks; - /// The quality of service profile to pass on to the rmw implementation. - rmw_qos_profile_t qos_profile = rmw_qos_profile_default; /// True to ignore local publications. bool ignore_local_publications = false; /// The callback group for this subscription. NULL to use the default callback group. - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; - /// Optional custom allocator. - std::shared_ptr allocator; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr; /// Setting to explicitly set intraprocess communications. IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; }; +/// Structure containing optional configuration for Subscriptions. +template +struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase +{ + /// Optional custom allocator. + std::shared_ptr allocator = nullptr; + + SubscriptionOptionsWithAllocator() {} + + /// Constructor using base class as input. + explicit SubscriptionOptionsWithAllocator( + const SubscriptionOptionsBase & subscription_options_base) + : SubscriptionOptionsBase(subscription_options_base) + {} + + /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t. + template + rcl_subscription_options_t + to_rcl_subscription_options(const rclcpp::QoS & qos) const + { + rcl_subscription_options_t result; + using AllocatorTraits = std::allocator_traits; + using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; + auto message_alloc = std::make_shared(*allocator.get()); + result.allocator = allocator::get_rcl_allocator(*message_alloc); + result.ignore_local_publications = this->ignore_local_publications; + result.qos = qos.get_rmw_qos_profile(); + return result; + } +}; + using SubscriptionOptions = SubscriptionOptionsWithAllocator>; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 3a73228f5e..ecab458bd7 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -18,9 +18,13 @@ #include #include "rclcpp/function_traits.hpp" +#include "rcl/types.h" namespace rclcpp { + +class QoS; + namespace subscription_traits { @@ -69,7 +73,20 @@ template struct extract_message_type>: extract_message_type {}; -template +template< + typename CallbackT, + // Do not attempt if CallbackT is an integer (mistaken for depth) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a QoS (mistaken for qos) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile) + typename = std::enable_if_t>>::value> +> struct has_message_type : extract_message_type< typename rclcpp::function_traits::function_traits::template argument_type<0>> {}; diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index cb84e05d89..ca6007fe88 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -65,10 +65,6 @@ Duration::Duration(const rcl_duration_t & duration) // noop } -Duration::~Duration() -{ -} - Duration::operator builtin_interfaces::msg::Duration() const { builtin_interfaces::msg::Duration msg_duration; @@ -226,4 +222,15 @@ Duration::seconds() const return std::chrono::duration(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count(); } +rmw_time_t +Duration::to_rmw_time() const +{ + // reuse conversion logic from msg creation + builtin_interfaces::msg::Duration msg = *this; + rmw_time_t result; + result.sec = msg.sec; + result.nsec = msg.nanosec; + return result; +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 8313932621..f008ecabef 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -109,7 +109,11 @@ Node::Node( const std::string & namespace_, const NodeOptions & options) : node_base_(new rclcpp::node_interfaces::NodeBase( - node_name, namespace_, options)), + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), @@ -129,10 +133,10 @@ Node::Node( node_services_, node_clock_, options.initial_parameters(), - options.use_intra_process_comms(), options.start_parameter_services(), options.start_parameter_event_publisher(), - options.parameter_event_qos_profile(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), options.automatically_declare_initial_parameters() )), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index a9198ccd73..9241adfd6a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -32,8 +32,11 @@ using rclcpp::node_interfaces::NodeBase; NodeBase::NodeBase( const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options) -: context_(options.context()), + rclcpp::Context::SharedPtr context, + const rcl_node_options_t & rcl_node_options, + bool use_intra_process_default) +: context_(context), + use_intra_process_default_(use_intra_process_default), node_handle_(nullptr), default_callback_group_(nullptr), associated_with_executor_(false), @@ -42,7 +45,7 @@ NodeBase::NodeBase( // Setup the guard condition that is notified when changes occur in the graph. rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); rcl_ret_t ret = rcl_guard_condition_init( - ¬ify_guard_condition_, options.context()->get_rcl_context().get(), guard_condition_options); + ¬ify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); if (ret != RCL_RET_OK) { throw_from_rcl_error(ret, "failed to create interrupt guard condition"); } @@ -63,7 +66,7 @@ NodeBase::NodeBase( ret = rcl_node_init( rcl_node.get(), node_name.c_str(), namespace_.c_str(), - options.context()->get_rcl_context().get(), options.get_rcl_node_options()); + context_->get_rcl_context().get(), &rcl_node_options); if (ret != RCL_RET_OK) { // Finalize the interrupt guard condition. finalize_notify_guard_condition(); @@ -259,3 +262,9 @@ NodeBase::acquire_notify_guard_condition_lock() const { return std::unique_lock(notify_guard_condition_mutex_); } + +bool +NodeBase::get_use_intra_process_default() const +{ + return use_intra_process_default_; +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index fea2e87842..d0a1fc403b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -45,14 +45,14 @@ using rclcpp::node_interfaces::NodeParameters; NodeParameters::NodeParameters( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, const std::vector & initial_parameters, - bool use_intra_process, bool start_parameter_services, bool start_parameter_event_publisher, - const rmw_qos_profile_t & parameter_event_qos_profile, + const rclcpp::QoS & parameter_event_qos, + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, bool allow_undeclared_parameters, bool automatically_declare_initial_parameters) : allow_undeclared_(allow_undeclared_parameters), @@ -64,7 +64,9 @@ NodeParameters::NodeParameters( using PublisherT = rclcpp::Publisher; using AllocatorT = std::allocator; // TODO(wjwwood): expose this allocator through the Parameter interface. - auto allocator = std::make_shared(); + rclcpp::PublisherOptionsWithAllocator publisher_options( + parameter_event_publisher_options); + publisher_options.allocator = std::make_shared(); if (start_parameter_services) { parameter_service_ = std::make_shared(node_base, node_services, this); @@ -72,13 +74,10 @@ NodeParameters::NodeParameters( if (start_parameter_event_publisher) { events_publisher_ = rclcpp::create_publisher( - node_topics.get(), + node_topics, "parameter_events", - parameter_event_qos_profile, - PublisherEventCallbacks(), - nullptr, - use_intra_process, - allocator); + parameter_event_qos, + publisher_options); } // Get the node options diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index e513464c1f..b133443d2d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -34,7 +34,7 @@ rclcpp::PublisherBase::SharedPtr NodeTopics::create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, - rcl_publisher_options_t & publisher_options, + const rcl_publisher_options_t & publisher_options, bool use_intra_process) { // Create the MessageT specific Publisher using the factory, but store it as PublisherBase. @@ -91,7 +91,7 @@ rclcpp::SubscriptionBase::SharedPtr NodeTopics::create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, - rcl_subscription_options_t & subscription_options, + const rcl_subscription_options_t & subscription_options, bool use_intra_process) { auto subscription = subscription_factory.create_typed_subscription( @@ -103,8 +103,9 @@ NodeTopics::create_subscription( auto ipm = context->get_sub_context(); uint64_t intra_process_subscription_id = ipm->add_subscription(subscription); - subscription_options.ignore_local_publications = false; - subscription->setup_intra_process(intra_process_subscription_id, ipm, subscription_options); + auto options_copy = subscription_options; + options_copy.ignore_local_publications = false; + subscription->setup_intra_process(intra_process_subscription_id, ipm, options_copy); } // Return the completed subscription. @@ -142,3 +143,9 @@ NodeTopics::add_subscription( } } } + +rclcpp::node_interfaces::NodeBaseInterface * +NodeTopics::get_node_base_interface() const +{ + return node_base_; +} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 099596cbdf..c080f74d04 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -21,6 +21,8 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" using rclcpp::exceptions::throw_from_rcl_error; @@ -208,16 +210,30 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe return *this; } -const rmw_qos_profile_t & -NodeOptions::parameter_event_qos_profile() const +const rclcpp::QoS & +NodeOptions::parameter_event_qos() const { - return this->parameter_event_qos_profile_; + return this->parameter_event_qos_; } NodeOptions & -NodeOptions::parameter_event_qos_profile(const rmw_qos_profile_t & parameter_event_qos_profile) +NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos) { - this->parameter_event_qos_profile_ = parameter_event_qos_profile; + this->parameter_event_qos_ = parameter_event_qos; + return *this; +} + +const rclcpp::PublisherOptionsBase & +NodeOptions::parameter_event_publisher_options() const +{ + return parameter_event_publisher_options_; +} + +NodeOptions & +NodeOptions::parameter_event_publisher_options( + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options) +{ + parameter_event_publisher_options_ = parameter_event_publisher_options; return *this; } diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp new file mode 100644 index 0000000000..92108bfc66 --- /dev/null +++ b/rclcpp/src/rclcpp/qos.cpp @@ -0,0 +1,207 @@ +// Copyright 2019 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 "rclcpp/qos.hpp" + +#include + +namespace rclcpp +{ + +QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg) +: history_policy(history_policy_arg), depth(depth_arg) +{} + +QoSInitialization +QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) +{ + switch (rmw_qos.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + return KeepAll(); + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + default: + return KeepLast(rmw_qos.depth); + } +} + +KeepAll::KeepAll() +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) +{} + +KeepLast::KeepLast(size_t depth) +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) +{} + +QoS::QoS( + const QoSInitialization & qos_initialization, + const rmw_qos_profile_t & initial_profile) +: rmw_qos_profile_(initial_profile) +{ + rmw_qos_profile_.history = qos_initialization.history_policy; + rmw_qos_profile_.depth = qos_initialization.depth; +} + +QoS::QoS(size_t history_depth) +: QoS(KeepLast(history_depth)) +{} + +rmw_qos_profile_t & +QoS::get_rmw_qos_profile() +{ + return rmw_qos_profile_; +} + +const rmw_qos_profile_t & +QoS::get_rmw_qos_profile() const +{ + return rmw_qos_profile_; +} + +QoS & +QoS::history(rmw_qos_history_policy_t history) +{ + rmw_qos_profile_.history = history; + return *this; +} + +QoS & +QoS::keep_last(size_t depth) +{ + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + rmw_qos_profile_.depth = depth; + return *this; +} + +QoS & +QoS::keep_all() +{ + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + rmw_qos_profile_.depth = 0; + return *this; +} + +QoS & +QoS::reliability(rmw_qos_reliability_policy_t reliability) +{ + rmw_qos_profile_.reliability = reliability; + return *this; +} + +QoS & +QoS::reliable() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); +} + +QoS & +QoS::best_effort() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); +} + +QoS & +QoS::durability(rmw_qos_durability_policy_t durability) +{ + rmw_qos_profile_.durability = durability; + return *this; +} + +QoS & +QoS::volitile() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); +} + +QoS & +QoS::transient_local() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); +} + +QoS & +QoS::deadline(rmw_time_t deadline) +{ + rmw_qos_profile_.deadline = deadline; + return *this; +} + +QoS & +QoS::deadline(const rclcpp::Duration & deadline) +{ + return this->deadline(deadline.to_rmw_time()); +} + +QoS & +QoS::lifespan(rmw_time_t lifespan) +{ + rmw_qos_profile_.lifespan = lifespan; + return *this; +} + +QoS & +QoS::lifespan(const rclcpp::Duration & lifespan) +{ + return this->lifespan(lifespan.to_rmw_time()); +} + +QoS & +QoS::liveliness(rmw_qos_liveliness_policy_t liveliness) +{ + rmw_qos_profile_.liveliness = liveliness; + return *this; +} + +QoS & +QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration) +{ + rmw_qos_profile_.liveliness_lease_duration = liveliness_lease_duration; + return *this; +} + +QoS & +QoS::liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration) +{ + return this->liveliness_lease_duration(liveliness_lease_duration.to_rmw_time()); +} + +QoS & +QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) +{ + rmw_qos_profile_.avoid_ros_namespace_conventions = avoid_ros_namespace_conventions; + return *this; +} + +SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_sensor_data) +{} + +ParametersQoS::ParametersQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_parameters) +{} + +ServicesQoS::ServicesQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_services_default) +{} + +ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_parameter_events) +{} + +SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_system_default) +{} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index ca2ad27da2..035a6efc06 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -28,7 +28,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/time_source.hpp" - namespace rclcpp { @@ -208,27 +207,13 @@ void TimeSource::create_clock_sub() // Subscription already created. return; } - const std::string topic_name = "/clock"; - - rclcpp::callback_group::CallbackGroup::SharedPtr group; - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - auto msg_mem_strat = MessageMemoryStrategy::create_default(); - auto allocator = std::make_shared(); - auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - - clock_subscription_ = rclcpp::create_subscription< - MessageT, decltype(cb), Alloc, MessageT, SubscriptionT - >( - node_topics_.get(), - topic_name, - std::move(cb), - rmw_qos_profile_default, - rclcpp::SubscriptionEventCallbacks(), - group, - false, - false, - msg_mem_strat, - allocator); + + clock_subscription_ = rclcpp::create_subscription( + node_topics_, + "/clock", + rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)), + std::bind(&TimeSource::clock_cb, this, std::placeholders::_1) + ); } void TimeSource::destroy_clock_sub() diff --git a/rclcpp/test/node_interfaces/node_wrapper.hpp b/rclcpp/test/node_interfaces/node_wrapper.hpp new file mode 100644 index 0000000000..f4319f545d --- /dev/null +++ b/rclcpp/test/node_interfaces/node_wrapper.hpp @@ -0,0 +1,64 @@ +// Copyright 2019 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 NODE_INTERFACES__NODE_WRAPPER_HPP_ +#define NODE_INTERFACES__NODE_WRAPPER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +class NodeWrapper +{ +public: + explicit NodeWrapper(const std::string & name) + : node(std::make_shared(name)) + {} + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() {return this->node->get_node_base_interface();} + + rclcpp::node_interfaces::NodeClockInterface::SharedPtr + get_node_clock_interface() {return this->node->get_node_clock_interface();} + + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface() {return this->node->get_node_graph_interface();} + + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface() {return this->node->get_node_logging_interface();} + + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface() {return this->node->get_node_timers_interface();} + + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface() {return this->node->get_node_topics_interface();} + + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface() {return this->node->get_node_services_interface();} + + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface() {return this->node->get_node_waitables_interface();} + + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface() {return this->node->get_node_parameters_interface();} + + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface() {return this->node->get_node_time_source_interface();} + +private: + rclcpp::Node::SharedPtr node; +}; + +#endif // NODE_INTERFACES__NODE_WRAPPER_HPP_ diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp new file mode 100644 index 0000000000..121f64126f --- /dev/null +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +int main(void) +{ + auto node = std::make_shared("test_node"); + std::shared_ptr const_node_ptr = node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_ptr); + (void)result; +} diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp new file mode 100644 index 0000000000..4f5b8311af --- /dev/null +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp @@ -0,0 +1,30 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +#include "../node_wrapper.hpp" + +int main(void) +{ + auto node = std::make_shared("test_wrapped_node"); + std::shared_ptr const_node_ptr = node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_ptr); + (void)result; +} diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp new file mode 100644 index 0000000000..ed147418f5 --- /dev/null +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +int main(void) +{ + auto node = std::make_shared("test_node"); + const rclcpp::Node & const_node_reference = *node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_reference); + (void)result; +} diff --git a/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp new file mode 100644 index 0000000000..6d7c19cac7 --- /dev/null +++ b/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp @@ -0,0 +1,30 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +#include "../node_wrapper.hpp" + +int main(void) +{ + auto node = std::make_shared("test_wrapped_node"); + const NodeWrapper & const_node_reference = *node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_reference); + (void)result; +} diff --git a/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp b/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp new file mode 100644 index 0000000000..f41d40061b --- /dev/null +++ b/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp @@ -0,0 +1,97 @@ +// Copyright 2019 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 "gtest/gtest.h" + +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "./node_wrapper.hpp" + +static const std::string node_suffix = "test_get_node_interfaces"; // NOLINT + +class TestGetNodeInterfaces : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + node = std::make_shared(node_suffix); + wrapped_node = std::make_shared("wrapped_" + node_suffix); + } + + static void TearDownTestCase() + { + node.reset(); + wrapped_node.reset(); + } + + static rclcpp::Node::SharedPtr node; + static std::shared_ptr wrapped_node; +}; + +rclcpp::Node::SharedPtr TestGetNodeInterfaces::node = nullptr; +std::shared_ptr TestGetNodeInterfaces::wrapped_node = nullptr; + +TEST_F(TestGetNodeInterfaces, rclcpp_node_shared_ptr) { + auto result = rclcpp::node_interfaces::get_node_topics_interface(this->node); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, node_shared_ptr) { + auto result = rclcpp::node_interfaces::get_node_topics_interface(this->wrapped_node); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) { + rclcpp::Node & node_reference = *this->node; + auto result = rclcpp::node_interfaces::get_node_topics_interface(node_reference); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, node_reference) { + NodeWrapper & wrapped_node_reference = *this->wrapped_node; + auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_reference); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, interface_shared_pointer) { + std::shared_ptr interface_shared_ptr = + this->node->get_node_topics_interface(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(interface_shared_ptr); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} diff --git a/rclcpp/test/test_duration.cpp b/rclcpp/test/test_duration.cpp index 7d61c68a6d..2fbc701f6a 100644 --- a/rclcpp/test/test_duration.cpp +++ b/rclcpp/test/test_duration.cpp @@ -60,6 +60,7 @@ TEST(TestDuration, operators) { rclcpp::Duration time = rclcpp::Duration(0, 0); rclcpp::Duration copy_constructor_duration(time); rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); + (void)assignment_op_duration; assignment_op_duration = time; EXPECT_TRUE(time == copy_constructor_duration); @@ -75,6 +76,14 @@ TEST(TestDuration, chrono_overloads) { EXPECT_EQ(d1, d2); EXPECT_EQ(d1, d3); EXPECT_EQ(d2, d3); + + // check non-nanosecond durations + std::chrono::milliseconds chrono_ms(100); + auto d4 = rclcpp::Duration(chrono_ms); + EXPECT_EQ(chrono_ms, d4.to_chrono()); + std::chrono::duration chrono_float_seconds(3.14); + auto d5 = rclcpp::Duration(chrono_float_seconds); + EXPECT_EQ(chrono_float_seconds, d5.to_chrono()); } TEST(TestDuration, overflows) { diff --git a/rclcpp/test/test_pub_sub_option_interface.cpp b/rclcpp/test/test_pub_sub_option_interface.cpp deleted file mode 100644 index 0074e9cd40..0000000000 --- a/rclcpp/test/test_pub_sub_option_interface.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2019 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 "rclcpp/rclcpp.hpp" -#include "test_msgs/msg/empty.hpp" - -class TestPubSubOptionAPI : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node", "/ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -TEST_F(TestPubSubOptionAPI, check_for_ambiguous) { - rclcpp::PublisherOptions pub_options; - rclcpp::SubscriptionOptions sub_options; - - auto topic_only_pub = node->create_publisher( - "topic_only"); - auto topic_depth_pub = node->create_publisher( - "topic_depth", - 10); - auto all_options_pub = node->create_publisher( - "topic_options", - 10, - pub_options); - - auto topic_only_sub = node->create_subscription( - "topic_only", - [](std::shared_ptr) {}); - auto topic_depth_sub = node->create_subscription( - "topic_depth", - [](std::shared_ptr) {}, - 10); - auto all_options_sub = node->create_subscription( - "topic_options", - [](std::shared_ptr) {}, - 10, - sub_options); -} diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp index e0815dff32..bcad9e88ea 100644 --- a/rclcpp/test/test_publisher.cpp +++ b/rclcpp/test/test_publisher.cpp @@ -80,16 +80,88 @@ TEST_F(TestPublisher, construction_and_destruction) { initialize(); using rcl_interfaces::msg::IntraProcessMessage; { - auto publisher = node->create_publisher("topic"); + auto publisher = node->create_publisher("topic", 42); + (void)publisher; } { ASSERT_THROW({ - auto publisher = node->create_publisher("invalid_topic?"); + auto publisher = node->create_publisher("invalid_topic?", 42); }, rclcpp::exceptions::InvalidTopicNameError); } } +/* + Testing publisher creation signatures. + */ +TEST_F(TestPublisher, various_creation_signatures) { + initialize(); + using rcl_interfaces::msg::IntraProcessMessage; + { + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + { + auto publisher = node->create_publisher("topic", rclcpp::QoS(42)); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", rclcpp::QoS(rclcpp::KeepLast(42))); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", rclcpp::QoS(rclcpp::KeepAll())); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + auto publisher = + rclcpp::create_publisher(node, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + // Now deprecated functions. +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + { + auto publisher = node->create_publisher("topic"); + (void)publisher; + } + { + auto publisher = node->create_publisher( + "topic", + 42, + std::make_shared>()); + (void)publisher; + } + { + auto publisher = node->create_publisher("topic", rmw_qos_profile_default); + (void)publisher; + } + { + auto publisher = node->create_publisher( + "topic", + rmw_qos_profile_default, + std::make_shared>()); + (void)publisher; + } +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + /* Testing publisher with intraprocess enabled and invalid QoS */ @@ -98,9 +170,21 @@ TEST_F(TestPublisher, intraprocess_with_invalid_qos) { rmw_qos_profile_t qos = invalid_qos_profile(); using rcl_interfaces::msg::IntraProcessMessage; { +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_THROW( {auto publisher = node->create_publisher("topic", qos);}, rclcpp::exceptions::InvalidParametersException); +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif } } @@ -110,20 +194,20 @@ TEST_F(TestPublisher, intraprocess_with_invalid_qos) { TEST_F(TestPublisherSub, construction_and_destruction) { using rcl_interfaces::msg::IntraProcessMessage; { - auto publisher = subnode->create_publisher("topic"); + auto publisher = subnode->create_publisher("topic", 42); EXPECT_STREQ(publisher->get_topic_name(), "/ns/sub_ns/topic"); } { - auto publisher = subnode->create_publisher("/topic"); + auto publisher = subnode->create_publisher("/topic", 42); EXPECT_STREQ(publisher->get_topic_name(), "/topic"); } { ASSERT_THROW({ - auto publisher = subnode->create_publisher("invalid_topic?"); + auto publisher = subnode->create_publisher("invalid_topic?", 42); }, rclcpp::exceptions::InvalidTopicNameError); } } diff --git a/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp/test/test_publisher_subscription_count_api.cpp index 2bc6bc48fd..d92f8f3dd4 100644 --- a/rclcpp/test/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/test_publisher_subscription_count_api.cpp @@ -74,12 +74,12 @@ TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) "my_node", "/ns", parameters.node_options[0]); - auto publisher = node->create_publisher("/topic"); + auto publisher = node->create_publisher("/topic", 10); EXPECT_EQ(publisher->get_subscription_count(), 0u); EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); { - auto sub = node->create_subscription("/topic", &OnMessage); + auto sub = node->create_subscription("/topic", 10, &OnMessage); rclcpp::sleep_for(offset); EXPECT_EQ(publisher->get_subscription_count(), 1u); EXPECT_EQ( @@ -91,7 +91,7 @@ TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) "/ns", parameters.node_options[1]); auto another_sub = - another_node->create_subscription("/topic", &OnMessage); + another_node->create_subscription("/topic", 10, &OnMessage); rclcpp::sleep_for(offset); EXPECT_EQ(publisher->get_subscription_count(), 2u); diff --git a/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp/test/test_serialized_message_allocator.cpp index ae65f7f1eb..66f8939e1f 100644 --- a/rclcpp/test/test_serialized_message_allocator.cpp +++ b/rclcpp/test/test_serialized_message_allocator.cpp @@ -56,8 +56,8 @@ TEST(TestSerializedMessageAllocator, borrow_from_subscription) { auto node = std::make_shared("test_serialized_message_allocator_node"); std::shared_ptr sub = - node->create_subscription("~/dummy_topic", []( - std::shared_ptr test_msg) {(void) test_msg;}); + node->create_subscription("~/dummy_topic", 10, + [](std::shared_ptr test_msg) {(void) test_msg;}); auto msg0 = sub->create_serialized_message(); EXPECT_EQ(0u, msg0->buffer_capacity); diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index ab0e0d1326..20b0999da1 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -95,7 +95,7 @@ class SubscriptionClassNodeInheritance : public rclcpp::Node auto callback = std::bind( &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); using rcl_interfaces::msg::IntraProcessMessage; - auto sub = this->create_subscription("topic", callback); + auto sub = this->create_subscription("topic", 10, callback); } }; @@ -112,7 +112,7 @@ class SubscriptionClass auto node = std::make_shared("test_subscription_member_callback", "/ns"); auto callback = std::bind(&SubscriptionClass::OnMessage, this, std::placeholders::_1); using rcl_interfaces::msg::IntraProcessMessage; - auto sub = node->create_subscription("topic", callback); + auto sub = node->create_subscription("topic", 10, callback); } }; @@ -125,12 +125,12 @@ TEST_F(TestSubscription, construction_and_destruction) { (void)msg; }; { - auto sub = node->create_subscription("topic", callback); + auto sub = node->create_subscription("topic", 10, callback); } { ASSERT_THROW({ - auto sub = node->create_subscription("invalid_topic?", callback); + auto sub = node->create_subscription("invalid_topic?", 10, callback); }, rclcpp::exceptions::InvalidTopicNameError); } } @@ -144,17 +144,17 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { (void)msg; }; { - auto sub = subnode->create_subscription("topic", callback); + auto sub = subnode->create_subscription("topic", 1, callback); EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); } { - auto sub = subnode->create_subscription("/topic", callback); + auto sub = subnode->create_subscription("/topic", 1, callback); EXPECT_STREQ(sub->get_topic_name(), "/topic"); } { - auto sub = subnode->create_subscription("~/topic", callback); + auto sub = subnode->create_subscription("~/topic", 1, callback); std::string expected_topic_name = std::string(node->get_namespace()) + "/" + node->get_name() + "/topic"; EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); @@ -162,11 +162,81 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { { ASSERT_THROW({ - auto sub = node->create_subscription("invalid_topic?", callback); + auto sub = node->create_subscription("invalid_topic?", 1, callback); }, rclcpp::exceptions::InvalidTopicNameError); } } +/* + Testing subscription creation signatures. + */ +TEST_F(TestSubscription, various_creation_signatures) { + using rcl_interfaces::msg::IntraProcessMessage; + auto cb = [](rcl_interfaces::msg::IntraProcessMessage::SharedPtr) {}; + { + auto sub = node->create_subscription("topic", 1, cb); + (void)sub; + } + { + auto sub = node->create_subscription("topic", rclcpp::QoS(1), cb); + (void)sub; + } + { + auto sub = + node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); + (void)sub; + } + { + auto sub = + node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); + (void)sub; + } + { + auto sub = node->create_subscription( + "topic", 42, cb, rclcpp::SubscriptionOptions()); + (void)sub; + } + { + auto sub = rclcpp::create_subscription( + node, "topic", 42, cb, rclcpp::SubscriptionOptions()); + (void)sub; + } + // Now deprecated functions. +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + { + auto sub = node->create_subscription("topic", cb, 42); + (void)sub; + } + { + auto sub = node->create_subscription("topic", cb); + (void)sub; + } + { + auto sub = node->create_subscription("topic", cb, rmw_qos_profile_default); + (void)sub; + } + { + auto sub = + node->create_subscription("topic", cb, rmw_qos_profile_default, nullptr); + (void)sub; + } + { + auto sub = node->create_subscription("topic", cb, 42, nullptr); + (void)sub; + } +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + /* Testing subscriptions using std::bind. */ @@ -187,6 +257,6 @@ TEST_F(TestSubscription, callback_bind) { // Regression test for https://github.com/ros2/rclcpp/issues/479 where the TEST_F GTest macro // was interfering with rclcpp's `function_traits`. auto callback = std::bind(&TestSubscription::OnMessage, this, std::placeholders::_1); - auto sub = node->create_subscription("topic", callback); + auto sub = node->create_subscription("topic", 1, callback); } } diff --git a/rclcpp/test/test_subscription_publisher_count_api.cpp b/rclcpp/test/test_subscription_publisher_count_api.cpp index 287075149a..cdf1bf95d5 100644 --- a/rclcpp/test/test_subscription_publisher_count_api.cpp +++ b/rclcpp/test/test_subscription_publisher_count_api.cpp @@ -65,11 +65,11 @@ TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) "my_node", "/ns", node_options); - auto subscription = node->create_subscription("/topic", &OnMessage); + auto subscription = node->create_subscription("/topic", 10, &OnMessage); EXPECT_EQ(subscription->get_publisher_count(), 0u); { - auto pub = node->create_publisher("/topic"); + auto pub = node->create_publisher("/topic", 10); rclcpp::sleep_for(offset); EXPECT_EQ(subscription->get_publisher_count(), 1u); { @@ -78,7 +78,7 @@ TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) "/ns", node_options); auto another_pub = - another_node->create_publisher("/topic"); + another_node->create_publisher("/topic", 10); rclcpp::sleep_for(offset); EXPECT_EQ(subscription->get_publisher_count(), 2u); diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index 74d8ff0663..1d666e57cb 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -129,8 +129,7 @@ void trigger_clock_changes( std::shared_ptr clock, bool expect_time_update = true) { - auto clock_pub = node->create_publisher("clock", - rmw_qos_profile_default); + auto clock_pub = node->create_publisher("clock", 10); for (int i = 0; i < 5; ++i) { if (!rclcpp::ok()) { diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 29f85680c3..727d10b158 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -20,6 +20,7 @@ include_directories(include) set(${PROJECT_NAME}_SRCS src/client.cpp + src/qos.cpp src/server.cpp src/server_goal_handle.cpp src/types.cpp diff --git a/rclcpp_action/include/rclcpp_action/qos.hpp b/rclcpp_action/include/rclcpp_action/qos.hpp new file mode 100644 index 0000000000..2acfddbc10 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/qos.hpp @@ -0,0 +1,34 @@ +// Copyright 2019 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 RCLCPP_ACTION__QOS_HPP_ +#define RCLCPP_ACTION__QOS_HPP_ + +#include + +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ + +class DefaultActionStatusQoS : public rclcpp::QoS +{ +public: + RCLCPP_ACTION_PUBLIC + DefaultActionStatusQoS(); +}; + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__QOS_HPP_ diff --git a/rclcpp_action/src/qos.cpp b/rclcpp_action/src/qos.cpp new file mode 100644 index 0000000000..615b825185 --- /dev/null +++ b/rclcpp_action/src/qos.cpp @@ -0,0 +1,28 @@ +// Copyright 2019 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 + +namespace rclcpp_action +{ + +DefaultActionStatusQoS::DefaultActionStatusQoS() +: rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rcl_action_qos_profile_status_default)) +{ + this->get_rmw_qos_profile() = rcl_action_qos_profile_status_default; +} + +} // namespace rclcpp_action diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index bae1720af1..117ddea241 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -42,6 +42,7 @@ #include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_action/client.hpp" +#include "rclcpp_action/qos.hpp" #include "rclcpp_action/types.hpp" using namespace std::chrono_literals; @@ -196,7 +197,8 @@ class TestClient : public ::testing::Test ret = rcl_action_get_feedback_topic_name( action_name, allocator, &feedback_topic_name); ASSERT_EQ(RCL_RET_OK, ret); - feedback_publisher = server_node->create_publisher(feedback_topic_name); + feedback_publisher = + server_node->create_publisher(feedback_topic_name, 10); ASSERT_TRUE(feedback_publisher != nullptr); allocator.deallocate(feedback_topic_name, allocator.state); @@ -205,7 +207,7 @@ class TestClient : public ::testing::Test action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); status_publisher = server_node->create_publisher( - status_topic_name, rcl_action_qos_profile_status_default); + status_topic_name, rclcpp_action::DefaultActionStatusQoS()); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 0fe664be61..03159a9f0f 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -367,7 +367,8 @@ TEST_F(TestServer, publish_status_accepted) // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -428,7 +429,8 @@ TEST_F(TestServer, publish_status_canceling) // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -483,7 +485,8 @@ TEST_F(TestServer, publish_status_canceled) // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -540,7 +543,8 @@ TEST_F(TestServer, publish_status_succeeded) // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -595,7 +599,8 @@ TEST_F(TestServer, publish_status_aborted) // Subscribe to status messages std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) { received_msgs.push_back(list); }); @@ -651,7 +656,7 @@ TEST_F(TestServer, publish_feedback) using FeedbackT = Fibonacci::Impl::FeedbackMessage; std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg) + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg) { received_msgs.push_back(msg); }); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 32459d3593..7779ca1d41 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -49,8 +49,11 @@ #include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" @@ -63,6 +66,29 @@ namespace rclcpp_lifecycle { +// include these here to work around an esoteric Windows error where the namespace +// cannot be used in the function declaration below without getting an error like: +// 'rclcpp::SubscriptionOptionsWithAllocator': +// no appropriate default constructor available +template +using PublisherOptionsWithAllocator = rclcpp::PublisherOptionsWithAllocator; +template +using SubscriptionOptionsWithAllocator = rclcpp::SubscriptionOptionsWithAllocator; + +template +PublisherOptionsWithAllocator +create_default_publisher_options() +{ + return rclcpp::PublisherOptionsWithAllocator(); +} + +template +SubscriptionOptionsWithAllocator +create_default_subscription_options() +{ + return rclcpp::SubscriptionOptionsWithAllocator(); +} + /// LifecycleNode for creating lifecycle components /** * has lifecycle nodeinterface for configuring this node. @@ -128,6 +154,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const std::vector & get_callback_groups() const; + /// Create and return a Publisher. + /** + * \param[in] topic_name The topic for this publisher to publish on. + * \param[in] qos The Quality of Service settings for this publisher. + * \param[in] options The publisher options for this publisher. + * \return Shared pointer to the created lifecycle publisher. + */ + template> + std::shared_ptr> + create_publisher( + const std::string & topic_name, + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options = + create_default_publisher_options() + ); + /// Create and return a Publisher. /** * \param[in] topic_name The topic for this publisher to publish on. @@ -136,10 +178,13 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \return Shared pointer to the created publisher. */ template> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function + [[deprecated("use create_publisher(const std::string &, const rclcpp::QoS &, ...) instead")]] std::shared_ptr> create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator = nullptr); + const std::string & topic_name, + size_t qos_history_depth, + std::shared_ptr allocator); /// Create and return a LifecyclePublisher. /** @@ -149,12 +194,44 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \return Shared pointer to the created publisher. */ template> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function + [[deprecated("use create_publisher(const std::string &, const rclcpp::QoS &, ...) instead")]] std::shared_ptr> create_publisher( const std::string & topic_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, std::shared_ptr allocator = nullptr); + /// Create and return a Subscription. + /** + * \param[in] topic_name The topic to subscribe on. + * \param[in] callback The user-defined callback function. + * \param[in] qos The quality of service for this subscription. + * \param[in] options The subscription options for this subscription. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \return Shared pointer to the created subscription. + */ + /* TODO(jacquelinekay): + Windows build breaks when static member function passed as default + argument to msg_mem_strat, nullptr is a workaround. + */ + template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription> + std::shared_ptr + create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const SubscriptionOptionsWithAllocator & options = + create_default_subscription_options(), + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + typename rclcpp::subscription_traits::has_message_type::type, AllocatorT + >::SharedPtr + msg_mem_strat = nullptr); + /// Create and return a Subscription. /** * \param[in] topic_name The topic to subscribe on. @@ -174,6 +251,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename CallbackT, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function + [[deprecated( + "use create_subscription(const std::string &, const rclcpp::QoS &, CallbackT, ...) instead" + )]] std::shared_ptr create_subscription( const std::string & topic_name, @@ -206,14 +287,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename CallbackT, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription> + // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function + [[deprecated( + "use create_subscription(const std::string &, const rclcpp::QoS &, CallbackT, ...) instead" + )]] std::shared_ptr create_subscription( const std::string & topic_name, size_t qos_history_depth, CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications = false, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr msg_mem_strat = nullptr, std::shared_ptr allocator = nullptr); @@ -457,6 +543,11 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface(); + /// Return the NodeOptions used when creating this node. + RCLCPP_LIFECYCLE_PUBLIC + const rclcpp::NodeOptions & + get_node_options() const; + // // LIFECYCLE COMPONENTS // @@ -585,7 +676,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; - bool use_intra_process_comms_; + const rclcpp::NodeOptions node_options_; class LifecycleNodeInterfaceImpl; std::unique_ptr impl_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index dc19f3c5b2..aeab685540 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -39,18 +39,34 @@ namespace rclcpp_lifecycle { +template +std::shared_ptr> +LifecycleNode::create_publisher( + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + using PublisherT = rclcpp_lifecycle::LifecyclePublisher; + return rclcpp::create_publisher( + *this, + topic_name, + qos, + options); +} + template std::shared_ptr> LifecycleNode::create_publisher( - const std::string & topic_name, size_t qos_history_depth, + const std::string & topic_name, + size_t qos_history_depth, std::shared_ptr allocator) { - if (!allocator) { - allocator = std::make_shared(); - } - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_publisher(topic_name, qos, allocator); + rclcpp::PublisherOptionsWithAllocator options; + options.allocator = allocator; + return this->create_publisher( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), + options); } template @@ -60,21 +76,48 @@ LifecycleNode::create_publisher( const rmw_qos_profile_t & qos_profile, std::shared_ptr allocator) { - using PublisherT = rclcpp_lifecycle::LifecyclePublisher; + rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile)); + qos.get_rmw_qos_profile() = qos_profile; + + rclcpp::PublisherOptionsWithAllocator pub_options; + pub_options.allocator = allocator; - // create regular publisher in rclcpp::Node - return rclcpp::create_publisher( - this->node_topics_.get(), + return this->create_publisher( topic_name, - qos_profile, - rclcpp::PublisherEventCallbacks(), - nullptr, - use_intra_process_comms_, - allocator); + qos, + pub_options); } // TODO(karsten1987): Create LifecycleSubscriber -template +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename SubscriptionT> +std::shared_ptr +LifecycleNode::create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + typename rclcpp::subscription_traits::has_message_type::type, AllocatorT>::SharedPtr + msg_mem_strat) +{ + return rclcpp::create_subscription( + *this, + topic_name, + qos, + std::forward(callback), + options, + msg_mem_strat); +} + +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> std::shared_ptr LifecycleNode::create_subscription( const std::string & topic_name, @@ -87,28 +130,16 @@ LifecycleNode::create_subscription( msg_mem_strat, std::shared_ptr allocator) { - using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type::type; - - if (!allocator) { - allocator = std::make_shared(); - } + rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile)); + qos.get_rmw_qos_profile() = qos_profile; - if (!msg_mem_strat) { - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - msg_mem_strat = MessageMemoryStrategy::create_default(); - } + rclcpp::SubscriptionOptionsWithAllocator sub_options; + sub_options.callback_group = group; + sub_options.ignore_local_publications = ignore_local_publications; + sub_options.allocator = allocator; - return rclcpp::create_subscription( - this->node_topics_.get(), - topic_name, - std::forward(callback), - qos_profile, - rclcpp::SubscriptionEventCallbacks(), - group, - ignore_local_publications, - use_intra_process_comms_, - msg_mem_strat, - allocator); + return this->create_subscription( + topic_name, std::forward(callback), qos, sub_options, msg_mem_strat); } template< @@ -123,21 +154,22 @@ LifecycleNode::create_subscription( CallbackT && callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr msg_mem_strat, std::shared_ptr allocator) { - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_subscription( + rclcpp::SubscriptionOptionsWithAllocator sub_options; + sub_options.callback_group = group; + sub_options.ignore_local_publications = ignore_local_publications; + sub_options.allocator = allocator; + + return this->create_subscription( topic_name, std::forward(callback), - qos, - rclcpp::SubscriptionEventCallbacks(), - group, - ignore_local_publications, - msg_mem_strat, - allocator); + rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), + sub_options, + msg_mem_strat); } template diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index ad4503bc76..4c60f4188b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -68,7 +68,11 @@ LifecycleNode::LifecycleNode( const std::string & namespace_, const rclcpp::NodeOptions & options) : node_base_(new rclcpp::node_interfaces::NodeBase( - node_name, namespace_, options)), + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), @@ -88,10 +92,10 @@ LifecycleNode::LifecycleNode( node_services_, node_clock_, options.initial_parameters(), - options.use_intra_process_comms(), options.start_parameter_services(), options.start_parameter_event_publisher(), - options.parameter_event_qos_profile(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), options.automatically_declare_initial_parameters() )), @@ -105,7 +109,7 @@ LifecycleNode::LifecycleNode( node_parameters_ )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), - use_intra_process_comms_(options.use_intra_process_comms()), + node_options_(options), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) { impl_->init(); @@ -333,6 +337,11 @@ LifecycleNode::get_node_waitables_interface() return node_waitables_; } +const rclcpp::NodeOptions & +LifecycleNode::get_node_options() const +{ + return node_options_; +} //// bool diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 98ba69123e..1ce941cad2 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -217,7 +217,8 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); auto cb = [](const std::shared_ptr msg) {(void) msg;}; - auto lifecycle_sub = test_node->create_subscription("~/empty", cb); + auto lifecycle_sub = + test_node->create_subscription("~/empty", 10, cb); SUCCEED(); }