From 9ecb2c323161823615803f166aeac12d001dc94e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 23 Oct 2017 11:06:37 -0700 Subject: [PATCH 01/17] publish_raw function --- rclcpp/include/rclcpp/publisher.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f505bef7a5..085ec280b5 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -279,6 +279,22 @@ class Publisher : public PublisherBase return this->publish(*msg); } + virtual void + publish(const rcl_message_raw_t * raw_msg) + { + if (store_intra_process_message_) { + // not supported atm + throw std::runtime_error("storing raw messages in intra process is not supported yet."); + } + auto status = rcl_publish_raw(&publisher_handle_, raw_msg); + if (status != RCL_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("failed to publish raw message: ") + rcl_get_error_string_safe()); + // *INDENT-ON* + } + } + std::shared_ptr get_allocator() const { return message_allocator_; From 0e1e7abc636d87554568ab702ee679f4b0eb7738 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 31 Oct 2017 16:39:54 -0700 Subject: [PATCH 02/17] subscription traits --- rclcpp/include/rclcpp/subscription_traits.hpp | 71 +++++++++++++++ rclcpp/test/test_subscription_traits.cpp | 89 +++++++++++++++++++ 2 files changed, 160 insertions(+) create mode 100644 rclcpp/include/rclcpp/subscription_traits.hpp create mode 100644 rclcpp/test/test_subscription_traits.cpp diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp new file mode 100644 index 0000000000..e24ad56ea7 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -0,0 +1,71 @@ +// Copyright 2017 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__SUBSCRIPTION_TRAITS_HPP_ +#define RCLCPP__SUBSCRIPTION_TRAITS_HPP_ + +#include "rclcpp/function_traits.hpp" + +namespace rclcpp +{ +namespace subscription_traits +{ + +template +struct is_raw_subscription_argument : std::false_type +{}; + +template<> +struct is_raw_subscription_argument : std::true_type +{}; + +template<> +struct is_raw_subscription_argument> : std::true_type +{}; + +template +struct is_raw_subscription : is_raw_subscription_argument +{}; + +template +struct is_raw_callback + : is_raw_subscription_argument< + typename rclcpp::function_traits::function_traits::template argument_type<0>> +{}; + +template +struct extract_message_type +{ + using type = MessageT; +}; + +template +struct extract_message_type> : extract_message_type +{}; + +template +struct extract_message_type> : extract_message_type +{}; + +template +struct has_message_type +{ + using type = extract_message_type< + typename rclcpp::function_traits::function_traits::template argument_type<0>>; +}; + +} // namespace subscription_traits +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_TRAITS_HPP_ diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp new file mode 100644 index 0000000000..afd0146fdf --- /dev/null +++ b/rclcpp/test/test_subscription_traits.cpp @@ -0,0 +1,89 @@ +// Copyright 2015 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 "rcl/types.h" + +#include "rclcpp/subscription_traits.hpp" + + +void raw_callback_copy(rcl_message_raw_t /* raw_copy */) +{} + +void raw_callback_shared_ptr(std::shared_ptr /* raw_shared_ptr */) +{} + +void not_raw_callback(char * /* char_ptr */) +{} + +void not_raw_shared_ptr_callback(std::shared_ptr /* shared_char_ptr */) +{} + +TEST(TestSubscriptionTraits, is_raw_callback) { + // Test regular functions + auto cb1 = &raw_callback_copy; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == true, + "rcl_message_raw_t in a first argument callback makes it a raw callback"); + + auto cb2 = &raw_callback_shared_ptr; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == true, + "std::shared_ptr in a first argument callback makes it a raw callback"); + + auto cb3 = ¬_raw_callback; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == false, + "passing a char * is not a raw callback"); + + auto cb4 = ¬_raw_shared_ptr_callback; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == false, + "passing a std::shared_tr is not a raw callback"); +} + +TEST(TestSubscriptionTraits, callback_messages) +{ + auto cb1 = &raw_callback_copy; + static_assert( + std::is_same< + rcl_message_raw_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "raw callback message type is rcl_message_raw_t"); + + auto cb2 = &raw_callback_shared_ptr; + static_assert( + std::is_same< + rcl_message_raw_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "raw callback message type is rcl_message_raw_t"); + + auto cb3 = ¬_raw_callback; + static_assert( + std::is_same< + char *, + rclcpp::subscription_traits::has_message_type::type>::value, + "not raw callback message type is char"); + + auto cb4 = ¬_raw_shared_ptr_callback; + static_assert( + std::is_same< + char, + rclcpp::subscription_traits::has_message_type::type>::value, + "not raw shared_ptr callback message type is std::shared_ptr"); +} From 84e6177791078d5751e797899abbdd13a535ef2d Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 10 Nov 2017 09:56:42 -0800 Subject: [PATCH 03/17] listener raw --- rclcpp/CMakeLists.txt | 7 +++ rclcpp/include/rclcpp/create_subscription.hpp | 15 ++++-- rclcpp/include/rclcpp/node.hpp | 15 ++++-- rclcpp/include/rclcpp/node_impl.hpp | 28 +++++++---- rclcpp/include/rclcpp/parameter_client.hpp | 4 +- rclcpp/include/rclcpp/publisher.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 46 ++++++++++++------- .../include/rclcpp/subscription_factory.hpp | 26 +++++++---- rclcpp/include/rclcpp/subscription_traits.hpp | 9 ++-- rclcpp/src/rclcpp/executor.cpp | 12 ++++- rclcpp/src/rclcpp/parameter_client.cpp | 3 +- rclcpp/src/rclcpp/subscription.cpp | 12 ++++- rclcpp/test/test_subscription_traits.cpp | 13 ++++++ .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 2 +- .../src/lifecycle_node_interface_impl.hpp | 9 ++-- 15 files changed, 142 insertions(+), 61 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3da0e466bd..48d2afc9c9 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -280,6 +280,13 @@ if(BUILD_TESTING) ) target_link_libraries(test_subscription ${PROJECT_NAME}) endif() + ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp) + if(TARGET test_subscription_traits) + target_include_directories(test_subscription_traits PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ) + endif() ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) target_include_directories(test_find_weak_nodes PUBLIC diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 5125d9b16f..59cadc108e 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -26,9 +26,14 @@ namespace rclcpp { -template -typename rclcpp::Subscription::SharedPtr -create_subscription( +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename CallbackMessageT, + typename SubscriptionT = rclcpp::Subscription> +typename std::shared_ptr +create_subscription_with_factory( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, const std::string & topic_name, CallbackT && callback, @@ -36,7 +41,7 @@ create_subscription( rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat, typename std::shared_ptr allocator) { @@ -45,7 +50,7 @@ create_subscription( subscription_options.ignore_local_publications = ignore_local_publications; auto factory = - rclcpp::create_subscription_factory( + rclcpp::create_subscription_factory( std::forward(callback), msg_mem_strat, allocator); auto sub = node_topics->create_subscription( diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 87974ef16b..1dfa12ef10 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -53,6 +53,7 @@ #include "rclcpp/publisher.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -183,7 +184,8 @@ class Node : public std::enable_shared_from_this typename MessageT, typename CallbackT, typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription> + typename SubscriptionT = rclcpp::Subscription< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>> std::shared_ptr create_subscription( const std::string & topic_name, @@ -191,7 +193,8 @@ class Node : public std::enable_shared_from_this const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, 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); @@ -214,15 +217,17 @@ class Node : public std::enable_shared_from_this typename MessageT, typename CallbackT, typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription> + typename SubscriptionT = rclcpp::Subscription< + typename rclcpp::subscription_traits::has_message_type::type, Alloc>> std::shared_ptr create_subscription( const std::string & topic_name, - size_t qos_history_depth, CallbackT && callback, + size_t qos_history_depth, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, 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); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e841f614f0..0a8f233284 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -82,7 +82,11 @@ Node::create_publisher( allocator); } -template +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> std::shared_ptr Node::create_subscription( const std::string & topic_name, @@ -90,20 +94,23 @@ Node::create_subscription( const rmw_qos_profile_t & qos_profile, 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) { + using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type::type; + if (!allocator) { allocator = std::make_shared(); } if (!msg_mem_strat) { using rclcpp::message_memory_strategy::MessageMemoryStrategy; - msg_mem_strat = MessageMemoryStrategy::create_default(); + msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription( + return rclcpp::create_subscription_with_factory( this->node_topics_.get(), topic_name, std::forward(callback), @@ -115,21 +122,26 @@ Node::create_subscription( allocator); } -template +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> std::shared_ptr Node::create_subscription( const std::string & topic_name, - size_t qos_history_depth, CallbackT && callback, + size_t qos_history_depth, 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( + return this->create_subscription( topic_name, std::forward(callback), qos, diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index cfd9a8e0ad..5ef1986c20 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -120,8 +120,8 @@ class AsyncParametersClient auto msg_mem_strat = MessageMemoryStrategy::create_default(); - return rclcpp::create_subscription< - rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, SubscriptionT>( + return rclcpp::create_subscription_with_factory< + rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, rcl_interfaces::msg::ParameterEvent, SubscriptionT>( this->node_topics_interface_.get(), "parameter_events", std::forward(callback), diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 085ec280b5..cf07b53dab 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -279,7 +279,7 @@ class Publisher : public PublisherBase return this->publish(*msg); } - virtual void + void publish(const rcl_message_raw_t * raw_msg) { if (store_intra_process_message_) { diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 91c1248452..37a2a2eff9 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,12 +29,13 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" -#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -64,7 +65,8 @@ class SubscriptionBase std::shared_ptr node_handle, const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, - const rcl_subscription_options_t & subscription_options); + const rcl_subscription_options_t & subscription_options, + bool is_raw = false); /// Default destructor. RCLCPP_PUBLIC @@ -109,6 +111,9 @@ class SubscriptionBase rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; + bool + is_raw() const; + protected: std::shared_ptr intra_process_subscription_handle_; std::shared_ptr subscription_handle_; @@ -116,19 +121,23 @@ class SubscriptionBase private: RCLCPP_DISABLE_COPY(SubscriptionBase) + + bool is_raw_; }; /// Subscription implementation, templated on the type of message this subscription receives. -template> +template< + typename CallbackMessageT, + typename Alloc = std::allocator> class Subscription : public SubscriptionBase { friend class rclcpp::node_interfaces::NodeTopicsInterface; public: - using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocTraits = allocator::AllocRebind; using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; + using MessageDeleter = allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -144,17 +153,19 @@ class Subscription : public SubscriptionBase */ Subscription( std::shared_ptr node_handle, + const rosidl_message_type_support_t & ts, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, - AnySubscriptionCallback callback, - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr - memory_strategy = message_memory_strategy::MessageMemoryStrategy callback, + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) : SubscriptionBase( node_handle, - *rosidl_typesupport_cpp::get_message_type_support_handle(), + ts, topic_name, - subscription_options), + subscription_options, + rclcpp::subscription_traits::is_raw_subscription_argument::value), any_callback_(callback), message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), @@ -167,11 +178,12 @@ class Subscription : public SubscriptionBase * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. */ void set_message_memory_strategy( - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) { message_memory_strategy_ = message_memory_strategy; } + std::shared_ptr create_message() { /* The default message memory strategy provides a dynamically allocated message on each call to @@ -190,7 +202,7 @@ class Subscription : public SubscriptionBase return; } } - auto typed_message = std::static_pointer_cast(message); + auto typed_message = std::static_pointer_cast(message); any_callback_.dispatch(typed_message, message_info); } @@ -198,7 +210,7 @@ class Subscription : public SubscriptionBase /** \param message message to be returned */ void return_message(std::shared_ptr & message) { - auto typed_message = std::static_pointer_cast(message); + auto typed_message = std::static_pointer_cast(message); message_memory_strategy_->return_message(typed_message); } @@ -278,8 +290,8 @@ class Subscription : public SubscriptionBase private: RCLCPP_DISABLE_COPY(Subscription) - AnySubscriptionCallback any_callback_; - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + AnySubscriptionCallback any_callback_; + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy_; GetMessageCallbackType get_intra_process_message_callback_; diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 024f32909f..e6a4c0bc38 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -25,6 +25,7 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/intra_process_manager.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/visibility_control.hpp" @@ -65,22 +66,28 @@ struct SubscriptionFactory }; /// Return a SubscriptionFactory with functions for creating a SubscriptionT. -template +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename CallbackMessageT, + typename SubscriptionT> SubscriptionFactory create_subscription_factory( CallbackT && callback, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat, std::shared_ptr allocator) { + SubscriptionFactory factory; using rclcpp::AnySubscriptionCallback; - AnySubscriptionCallback any_subscription_callback(allocator); + AnySubscriptionCallback any_subscription_callback(allocator); any_subscription_callback.set(std::forward(callback)); auto message_alloc = - std::make_shared::MessageAlloc>(); + std::make_shared::MessageAlloc>(); // factory function that creates a MessageT specific SubscriptionT factory.create_typed_subscription = @@ -91,13 +98,14 @@ create_subscription_factory( ) -> rclcpp::SubscriptionBase::SharedPtr { subscription_options.allocator = - rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); + rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); using rclcpp::Subscription; using rclcpp::SubscriptionBase; - auto sub = Subscription::make_shared( + auto sub = Subscription::make_shared( node_base->get_shared_rcl_node_handle(), + *rosidl_typesupport_cpp::get_message_type_support_handle(), topic_name, subscription_options, any_subscription_callback, @@ -117,7 +125,7 @@ create_subscription_factory( uint64_t intra_process_subscription_id = ipm->add_subscription(subscription); auto intra_process_options = rcl_subscription_get_default_options(); - intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( + intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( *message_alloc.get()); intra_process_options.qos = subscription_options.qos; intra_process_options.ignore_local_publications = false; @@ -128,7 +136,7 @@ create_subscription_factory( uint64_t publisher_id, uint64_t message_sequence, uint64_t subscription_id, - typename rclcpp::Subscription::MessageUniquePtr & message) + typename rclcpp::Subscription::MessageUniquePtr & message) { auto ipm = weak_ipm.lock(); if (!ipm) { @@ -136,7 +144,7 @@ create_subscription_factory( throw std::runtime_error( "intra process take called after destruction of intra process manager"); } - ipm->take_intra_process_message( + ipm->take_intra_process_message( publisher_id, message_sequence, subscription_id, message); }; diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index e24ad56ea7..83c98e7d8d 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -47,7 +47,7 @@ struct is_raw_callback template struct extract_message_type { - using type = MessageT; + using type = typename std::remove_cv::type; }; template @@ -60,10 +60,9 @@ struct extract_message_type> : extract_message_type struct has_message_type -{ - using type = extract_message_type< - typename rclcpp::function_traits::function_traits::template argument_type<0>>; -}; +: extract_message_type< + typename rclcpp::function_traits::function_traits::template argument_type<0>> +{}; } // namespace subscription_traits } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5cd79e9f1b..d820e858af 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -281,11 +281,21 @@ void Executor::execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription) { + // may have to overwrite create_message here std::shared_ptr message = subscription->create_message(); + fprintf(stderr, "Creating message at %p\n", message.get()); rmw_message_info_t message_info; - auto ret = rcl_take(subscription->get_subscription_handle().get(), + auto ret = RCL_RET_ERROR; + if (subscription->is_raw()) { + ret = rcl_take_raw(subscription->get_subscription_handle(), + reinterpret_cast(message.get()), &message_info); + auto rcl_ptr = message.get(); + (void) rcl_ptr; + } else { + ret = rcl_take(subscription->get_subscription_handle(), message.get(), &message_info); + } if (ret == RCL_RET_OK) { message_info.from_intra_process = false; subscription->handle_message(message, message_info); diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 3ba62ffb96..be3666a524 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -399,8 +399,9 @@ SyncParametersClient::set_parameters( { auto f = async_parameters_client_->set_parameters(parameters); + auto node_base_interface = node_->get_node_base_interface(); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == + if (spin_node_until_future_complete(*executor_, node_base_interface, f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 83401c0e80..5241b7fe18 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -31,8 +31,10 @@ SubscriptionBase::SubscriptionBase( std::shared_ptr node_handle, const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, - const rcl_subscription_options_t & subscription_options) -: node_handle_(node_handle) + const rcl_subscription_options_t & subscription_options, + bool is_raw) +: node_handle_(node_handle), + is_raw_(is_raw) { std::weak_ptr weak_node_handle(node_handle_); auto custom_deletor = [weak_node_handle](rcl_subscription_t * rcl_subs) @@ -111,3 +113,9 @@ SubscriptionBase::get_intra_process_subscription_handle() const { return intra_process_subscription_handle_; } + +bool +SubscriptionBase::is_raw() const +{ + return is_raw_; +} diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index afd0146fdf..e0906d16a8 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -59,6 +59,19 @@ TEST(TestSubscriptionTraits, is_raw_callback) { TEST(TestSubscriptionTraits, callback_messages) { + static_assert( + std::is_same< + std::shared_ptr, + rclcpp::function_traits::function_traits::template argument_type<0>>::value, "wrong!"); + + static_assert( + std::is_same< + char, + rclcpp::subscription_traits::extract_message_type< + rclcpp::function_traits::function_traits::template argument_type<0> + >::type + >::value, "wrong!1"); + auto cb1 = &raw_callback_copy; static_assert( std::is_same< diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 6b683e4a2a..000ec05708 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -89,7 +89,7 @@ LifecycleNode::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription< + return rclcpp::create_subscription_with_factory< MessageT, CallbackT, Alloc, rclcpp::Subscription>( this->node_topics_.get(), diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index b3c947ef51..3aba884f7f 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -297,11 +297,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return RCL_RET_ERROR; } + constexpr bool publish_update = false; // keep the initial state to pass to a transition callback State initial_state(state_machine_.current_state); uint8_t transition_id = lifecycle_transition; - if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s", transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()) return RCL_RET_ERROR; @@ -311,7 +312,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl state_machine_.current_state->id, initial_state); if (rcl_lifecycle_trigger_transition( - &state_machine_, cb_return_code, true) != RCL_RET_OK) + &state_machine_, cb_return_code, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s", transition_id, state_machine_.current_state->label) @@ -326,13 +327,13 @@ class LifecycleNode::LifecycleNodeInterfaceImpl state_machine_.current_state->id, initial_state); if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { // We call cleanup on the error state - if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR("Failed to call cleanup on error state") return RCL_RET_ERROR; } } else { // We call shutdown on the error state - if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, publish_update) != RCL_RET_OK) { RCUTILS_LOG_ERROR("Failed to call cleanup on error state") return RCL_RET_ERROR; } From 3cd94a55684080f0a8116e9f0e8d689f77996c9b Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 18 Jan 2018 16:32:07 -0800 Subject: [PATCH 04/17] rebased --- rclcpp/src/rclcpp/time_source.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 3a09716d55..e11df8219a 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -74,7 +74,7 @@ void TimeSource::attachNode( auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - clock_subscription_ = rclcpp::create_subscription( + clock_subscription_ = rclcpp::create_subscription_with_factory( node_topics_.get(), topic_name, std::move(cb), From 50a4fc9d2d97b1c5e40ae76ff4ec5b9dc5f937a0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Feb 2018 16:08:10 -0800 Subject: [PATCH 05/17] cleanup and linters --- rclcpp/include/rclcpp/create_subscription.hpp | 9 ++-- rclcpp/include/rclcpp/node_impl.hpp | 2 +- rclcpp/include/rclcpp/parameter_client.hpp | 5 ++- .../include/rclcpp/subscription_factory.hpp | 4 +- rclcpp/include/rclcpp/subscription_traits.hpp | 21 +++++++--- rclcpp/src/rclcpp/executor.cpp | 7 ++-- rclcpp/src/rclcpp/time_source.cpp | 3 +- rclcpp/test/test_subscription_traits.cpp | 41 ++++++++++++------- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 2 +- .../src/lifecycle_node_interface_impl.hpp | 13 ++++-- 10 files changed, 70 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 59cadc108e..a4ea581092 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -33,7 +33,7 @@ template< typename CallbackMessageT, typename SubscriptionT = rclcpp::Subscription> typename std::shared_ptr -create_subscription_with_factory( +create_subscription( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, const std::string & topic_name, CallbackT && callback, @@ -41,7 +41,8 @@ create_subscription_with_factory( rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, AllocatorT>::SharedPtr msg_mem_strat, typename std::shared_ptr allocator) { @@ -49,8 +50,8 @@ create_subscription_with_factory( subscription_options.qos = qos_profile; subscription_options.ignore_local_publications = ignore_local_publications; - auto factory = - rclcpp::create_subscription_factory( + auto factory = rclcpp::create_subscription_factory + ( std::forward(callback), msg_mem_strat, allocator); auto sub = node_topics->create_subscription( diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 0a8f233284..716266f35f 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -110,7 +110,7 @@ Node::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription_with_factory( + return rclcpp::create_subscription( this->node_topics_.get(), topic_name, std::forward(callback), diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 5ef1986c20..ac3250bf8d 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -120,8 +120,9 @@ class AsyncParametersClient auto msg_mem_strat = MessageMemoryStrategy::create_default(); - return rclcpp::create_subscription_with_factory< - rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, rcl_interfaces::msg::ParameterEvent, SubscriptionT>( + using rcl_interfaces::msg::ParameterEvent; + return rclcpp::create_subscription< + ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>( this->node_topics_interface_.get(), "parameter_events", std::forward(callback), diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index e6a4c0bc38..66b452ded6 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -75,11 +75,11 @@ template< SubscriptionFactory create_subscription_factory( CallbackT && callback, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, Alloc>::SharedPtr msg_mem_strat, std::shared_ptr allocator) { - SubscriptionFactory factory; using rclcpp::AnySubscriptionCallback; diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 83c98e7d8d..91738d220a 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__SUBSCRIPTION_TRAITS_HPP_ #define RCLCPP__SUBSCRIPTION_TRAITS_HPP_ +#include + #include "rclcpp/function_traits.hpp" namespace rclcpp @@ -22,16 +24,24 @@ namespace rclcpp namespace subscription_traits { +/* + * The current version of uncrustify has a misinterpretion here + * between `:` used for inheritance vs for initializer list + * The result is that whenever a templated struct is used, + * the colon has to be without any whitespace next to it whereas + * when no template is used, the colon has to be separated by a space. + * Cheers! + */ template struct is_raw_subscription_argument : std::false_type {}; template<> -struct is_raw_subscription_argument : std::true_type +struct is_raw_subscription_argument: std::true_type {}; template<> -struct is_raw_subscription_argument> : std::true_type +struct is_raw_subscription_argument>: std::true_type {}; template @@ -51,16 +61,15 @@ struct extract_message_type }; template -struct extract_message_type> : extract_message_type +struct extract_message_type>: extract_message_type {}; template -struct extract_message_type> : extract_message_type +struct extract_message_type>: extract_message_type {}; template -struct has_message_type -: extract_message_type< +struct has_message_type : extract_message_type< typename rclcpp::function_traits::function_traits::template argument_type<0>> {}; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index d820e858af..84656cf448 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -283,17 +283,18 @@ Executor::execute_subscription( { // may have to overwrite create_message here std::shared_ptr message = subscription->create_message(); - fprintf(stderr, "Creating message at %p\n", message.get()); rmw_message_info_t message_info; auto ret = RCL_RET_ERROR; if (subscription->is_raw()) { - ret = rcl_take_raw(subscription->get_subscription_handle(), + ret = rcl_take_raw( + subscription->get_subscription_handle(), reinterpret_cast(message.get()), &message_info); auto rcl_ptr = message.get(); (void) rcl_ptr; } else { - ret = rcl_take(subscription->get_subscription_handle(), + ret = rcl_take( + subscription->get_subscription_handle(), message.get(), &message_info); } if (ret == RCL_RET_OK) { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index e11df8219a..e492b64b34 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -74,7 +74,8 @@ void TimeSource::attachNode( auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - clock_subscription_ = rclcpp::create_subscription_with_factory( + clock_subscription_ = rclcpp::create_subscription< + MessageT, decltype(cb), Alloc, MessageT, SubscriptionT>( node_topics_.get(), topic_name, std::move(cb), diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index e0906d16a8..50f85f0b5d 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include "rcl/types.h" @@ -22,17 +23,25 @@ #include "rclcpp/subscription_traits.hpp" -void raw_callback_copy(rcl_message_raw_t /* raw_copy */) -{} +void raw_callback_copy(rcl_message_raw_t unused) +{ + (void) unused; +} -void raw_callback_shared_ptr(std::shared_ptr /* raw_shared_ptr */) -{} +void raw_callback_shared_ptr(std::shared_ptr unused) +{ + (void) unused; +} -void not_raw_callback(char * /* char_ptr */) -{} +void not_raw_callback(char * unused) +{ + (void) unused; +} -void not_raw_shared_ptr_callback(std::shared_ptr /* shared_char_ptr */) -{} +void not_raw_shared_ptr_callback(std::shared_ptr unused) +{ + (void) unused; +} TEST(TestSubscriptionTraits, is_raw_callback) { // Test regular functions @@ -57,20 +66,24 @@ TEST(TestSubscriptionTraits, is_raw_callback) { "passing a std::shared_tr is not a raw callback"); } -TEST(TestSubscriptionTraits, callback_messages) -{ +TEST(TestSubscriptionTraits, callback_messages) { static_assert( std::is_same< std::shared_ptr, - rclcpp::function_traits::function_traits::template argument_type<0>>::value, "wrong!"); + rclcpp::function_traits::function_traits< + decltype(not_raw_shared_ptr_callback) + >::template argument_type<0> + >::value, "wrong!"); static_assert( std::is_same< char, rclcpp::subscription_traits::extract_message_type< - rclcpp::function_traits::function_traits::template argument_type<0> - >::type - >::value, "wrong!1"); + rclcpp::function_traits::function_traits< + decltype(not_raw_shared_ptr_callback) + >::template argument_type<0> + >::type + >::value, "wrong!"); auto cb1 = &raw_callback_copy; static_assert( diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 000ec05708..6b683e4a2a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -89,7 +89,7 @@ LifecycleNode::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription_with_factory< + return rclcpp::create_subscription< MessageT, CallbackT, Alloc, rclcpp::Subscription>( this->node_topics_.get(), diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 3aba884f7f..38876110b8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -297,12 +297,15 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return RCL_RET_ERROR; } + // TODO(karsten1987): Set this back to true whenever we have c-raw publisher constexpr bool publish_update = false; // keep the initial state to pass to a transition callback State initial_state(state_machine_.current_state); uint8_t transition_id = lifecycle_transition; - if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, publish_update) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s", transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()) return RCL_RET_ERROR; @@ -327,13 +330,17 @@ class LifecycleNode::LifecycleNodeInterfaceImpl state_machine_.current_state->id, initial_state); if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { // We call cleanup on the error state - if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, publish_update) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition( + &state_machine_, error_resolved, publish_update) != RCL_RET_OK) + { RCUTILS_LOG_ERROR("Failed to call cleanup on error state") return RCL_RET_ERROR; } } else { // We call shutdown on the error state - if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, publish_update) != RCL_RET_OK) { + if (rcl_lifecycle_trigger_transition( + &state_machine_, error_resolved, publish_update) != RCL_RET_OK) + { RCUTILS_LOG_ERROR("Failed to call cleanup on error state") return RCL_RET_ERROR; } From 2723826b0ed5b1e43cddc62c88e1d8d0c45183fc Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 5 Mar 2018 14:27:59 -0800 Subject: [PATCH 06/17] explicit test for deleter in unique_ptr --- rclcpp/CMakeLists.txt | 5 +- rclcpp/include/rclcpp/subscription_traits.hpp | 4 +- rclcpp/package.xml | 1 + rclcpp/test/test_subscription_traits.cpp | 77 +++++++++++++++---- 4 files changed, 70 insertions(+), 17 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 48d2afc9c9..0bb9e25357 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -280,12 +280,15 @@ if(BUILD_TESTING) ) target_link_libraries(test_subscription ${PROJECT_NAME}) endif() + find_package(test_msgs REQUIRED) ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp) if(TARGET test_subscription_traits) target_include_directories(test_subscription_traits PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} ${rcl_INCLUDE_DIRS} ) + ament_target_dependencies(test_subscription_traits + "test_msgs" + ) endif() ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 91738d220a..8151f3ef60 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -64,8 +64,8 @@ template struct extract_message_type>: extract_message_type {}; -template -struct extract_message_type>: extract_message_type +template +struct extract_message_type>: extract_message_type {}; template diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 6e4d4c7692..2ff16f9a03 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -36,6 +36,7 @@ ament_lint_common rmw rmw_implementation_cmake + test_msgs ament_cmake diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index 50f85f0b5d..eb444812b3 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -1,17 +1,3 @@ -// Copyright 2015 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 @@ -20,8 +6,10 @@ #include "rcl/types.h" +#include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/subscription_traits.hpp" +#include "test_msgs/msg/empty.hpp" void raw_callback_copy(rcl_message_raw_t unused) { @@ -43,6 +31,13 @@ void not_raw_shared_ptr_callback(std::shared_ptr unused) (void) unused; } +void not_raw_unique_ptr_callback( + test_msgs::msg::Empty::UniquePtrWithDeleter, + test_msgs::msg::Empty>> unused) +{ + (void) unused; +} + TEST(TestSubscriptionTraits, is_raw_callback) { // Test regular functions auto cb1 = &raw_callback_copy; @@ -64,6 +59,30 @@ TEST(TestSubscriptionTraits, is_raw_callback) { static_assert( rclcpp::subscription_traits::is_raw_callback::value == false, "passing a std::shared_tr is not a raw callback"); + + auto cb5 = [](rcl_message_raw_t unused) -> void + { + (void) unused; + }; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == true, + "rcl_message_raw_t in a first argument callback makes it a raw callback"); + + using MessageT = test_msgs::msg::Empty; + using MessageTAllocator = std::allocator; + using MessageTDeallocator = rclcpp::allocator::Deleter; + auto cb6 = [](MessageT::UniquePtrWithDeleter unique_msg_ptr) -> void + { + (void) unique_msg_ptr; + }; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == false, + "passing a std::unique_ptr of test_msgs::msg::Empty is not a raw callback"); + + auto cb7 = ¬_raw_unique_ptr_callback; + static_assert( + rclcpp::subscription_traits::is_raw_callback::value == false, + "passing a fancy unique_ptr of test_msgs::msg::Empty is not a raw callback"); } TEST(TestSubscriptionTraits, callback_messages) { @@ -112,4 +131,34 @@ TEST(TestSubscriptionTraits, callback_messages) { char, rclcpp::subscription_traits::has_message_type::type>::value, "not raw shared_ptr callback message type is std::shared_ptr"); + + auto cb5 = [](rcl_message_raw_t unused) -> void + { + (void) unused; + }; + static_assert( + std::is_same< + rcl_message_raw_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "raw callback message type is rcl_message_raw_t"); + + using MessageT = test_msgs::msg::Empty; + using MessageTAllocator = std::allocator; + using MessageTDeallocator = rclcpp::allocator::Deleter; + auto cb6 = [](std::unique_ptr unique_msg_ptr) -> void + { + (void) unique_msg_ptr; + }; + static_assert( + std::is_same< + test_msgs::msg::Empty, + rclcpp::subscription_traits::has_message_type::type>::value, + "passing a std::unique_ptr of test_msgs::msg::Empty has message type Empty"); + + auto cb7 = ¬_raw_unique_ptr_callback; + static_assert( + std::is_same< + test_msgs::msg::Empty, + rclcpp::subscription_traits::has_message_type::type>::value, + "passing a fancy std::unique_ptr of test_msgs::msg::Empty has message type Empty"); } From ffb63132ff2412bb76dd39caec26320d812392c6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Mar 2018 09:38:11 -0800 Subject: [PATCH 07/17] add missing copyright --- rclcpp/test/test_subscription_traits.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index eb444812b3..bc122ab074 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -1,3 +1,17 @@ +// Copyright 2017 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 From 9d4c07a2aaecba224e6921f8cfa47a5ba1e20642 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Mar 2018 21:58:50 -0800 Subject: [PATCH 08/17] cleanup --- rclcpp/src/rclcpp/executor.cpp | 2 -- rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp | 3 +-- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 84656cf448..11d9592e20 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -290,8 +290,6 @@ Executor::execute_subscription( ret = rcl_take_raw( subscription->get_subscription_handle(), reinterpret_cast(message.get()), &message_info); - auto rcl_ptr = message.get(); - (void) rcl_ptr; } else { ret = rcl_take( subscription->get_subscription_handle(), diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 38876110b8..9fbe83e21a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -297,8 +297,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl return RCL_RET_ERROR; } - // TODO(karsten1987): Set this back to true whenever we have c-raw publisher - constexpr bool publish_update = false; + constexpr bool publish_update = true; // keep the initial state to pass to a transition callback State initial_state(state_machine_.current_state); From e1bf3d1945a5fa9c82df85c99783e2217f1bf305 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 23 Apr 2018 11:00:11 -0700 Subject: [PATCH 09/17] add rmw_serialize functions --- rclcpp/CMakeLists.txt | 11 ++++ .../rclcpp/message_memory_strategy.hpp | 58 +++++++++++++++++ rclcpp/include/rclcpp/subscription.hpp | 25 ++++++++ rclcpp/src/rclcpp/executor.cpp | 54 ++++++++++------ rclcpp/src/rclcpp/subscription.cpp | 7 +++ rclcpp/test/test_raw_message_allocator.cpp | 63 +++++++++++++++++++ 6 files changed, 198 insertions(+), 20 deletions(-) create mode 100644 rclcpp/test/test_raw_message_allocator.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 0bb9e25357..4eed7c372d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -260,6 +260,17 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() + ament_add_gtest(test_raw_message_allocator test/test_raw_message_allocator.cpp + ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}) + if(TARGET test_raw_message_allocator) + target_include_directories(test_raw_message_allocator PUBLIC + ${test_msgs_INCLUDE_DIRS} + ) + target_link_libraries(test_raw_message_allocator + ${PROJECT_NAME} + ${test_msgs_LIBRARIES} + ) + endif() ament_add_gtest(test_service test/test_service.cpp) if(TARGET test_service) target_include_directories(test_service PUBLIC diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index fafd0025ef..5d7046f6e5 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -18,10 +18,14 @@ #include #include +#include "rcl/types.h" + #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" +#include "rmw/raw_message.h" + namespace rclcpp { namespace message_memory_strategy @@ -39,14 +43,28 @@ class MessageMemoryStrategy using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = allocator::Deleter; + using RawMessageAllocTraits = allocator::AllocRebind; + using RawMessageAlloc = typename RawMessageAllocTraits::allocator_type; + using RawMessageDeleter = allocator::Deleter; + + using BufferAllocTraits = allocator::AllocRebind; + using BufferAlloc = typename BufferAllocTraits::allocator_type; + using BufferDeleter = allocator::Deleter; + MessageMemoryStrategy() { message_allocator_ = std::make_shared(); + raw_message_allocator_ = std::make_shared(); + buffer_allocator_ = std::make_shared(); + rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); } explicit MessageMemoryStrategy(std::shared_ptr allocator) { message_allocator_ = std::make_shared(*allocator.get()); + raw_message_allocator_ = std::make_shared(*allocator.get()); + buffer_allocator_ = std::make_shared(*allocator.get()); + rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); } /// Default factory method @@ -62,6 +80,31 @@ class MessageMemoryStrategy return std::allocate_shared(*message_allocator_.get()); } + virtual std::shared_ptr borrow_raw_message(unsigned int capacity) + { + // get rcutils_default allocator + // initialize raw message + // does this have to be a shared pointer ?! + //auto raw_msg = std::make_shared(rmw_get_zero_initialized_raw_message()); + auto raw_msg = std::make_shared(); + raw_msg->buffer_length = 0; + raw_msg->buffer_capacity = 0; + raw_msg->buffer = NULL; + rmw_initialize_raw_message(raw_msg.get(), 0, &rcutils_allocator_); + rmw_raw_message_resize(raw_msg.get(), capacity); + return raw_msg; + } + + virtual std::shared_ptr borrow_raw_message() + { + return borrow_raw_message(default_buffer_capacity_); + } + + virtual void set_default_buffer_capacity(unsigned int capacity) + { + default_buffer_capacity_ = capacity; + } + /// Release ownership of the message, which will deallocate it if it has no more owners. /** \param[in] msg Shared pointer to the message we are returning. */ virtual void return_message(std::shared_ptr & msg) @@ -69,8 +112,23 @@ class MessageMemoryStrategy msg.reset(); } + virtual void return_raw_message(std::shared_ptr & raw_msg) + { + rmw_raw_message_fini(raw_msg.get()); + raw_msg.reset(); + } + std::shared_ptr message_allocator_; MessageDeleter message_deleter_; + + std::shared_ptr raw_message_allocator_; + RawMessageDeleter raw_message_deleter_; + + std::shared_ptr buffer_allocator_; + BufferDeleter buffer_deleter_; + unsigned int default_buffer_capacity_ = 0; + + rcutils_allocator_t rcutils_allocator_; }; } // namespace message_memory_strategy diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 37a2a2eff9..da9717cd2c 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -93,6 +93,12 @@ class SubscriptionBase /** \return Shared pointer to the fresh message. */ virtual std::shared_ptr create_message() = 0; + + /// Borrow a new raw message + /** \return Shared pointer to a rcl_message_raw_t. */ + virtual std::shared_ptr + create_raw_message() = 0; + /// Check if we need to handle the message, and execute the callback if we do. /** * \param[in] message Shared pointer to the message to handle. @@ -106,11 +112,19 @@ class SubscriptionBase virtual void return_message(std::shared_ptr & message) = 0; + /// Return the message borrowed in create_raw_message. + /** \param[in] message Shared pointer to the returned message. */ + virtual void + return_raw_message(std::shared_ptr & message) = 0; + virtual void handle_intra_process_message( rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; + rosidl_message_type_support_t + get_message_type_support_handle() const; + bool is_raw() const; @@ -122,6 +136,7 @@ class SubscriptionBase private: RCLCPP_DISABLE_COPY(SubscriptionBase) + rosidl_message_type_support_t type_support_; bool is_raw_; }; @@ -193,6 +208,11 @@ class Subscription : public SubscriptionBase return message_memory_strategy_->borrow_message(); } + std::shared_ptr create_raw_message() + { + return message_memory_strategy_->borrow_raw_message(); + } + void handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) { if (matches_any_intra_process_publishers_) { @@ -214,6 +234,11 @@ class Subscription : public SubscriptionBase message_memory_strategy_->return_message(typed_message); } + void return_raw_message(std::shared_ptr & message) + { + message_memory_strategy_->return_raw_message(message); + } + void handle_intra_process_message( rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 11d9592e20..902eb4b202 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -281,31 +281,45 @@ void Executor::execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription) { - // may have to overwrite create_message here - std::shared_ptr message = subscription->create_message(); + auto raw_msg = subscription->create_raw_message(); rmw_message_info_t message_info; - auto ret = RCL_RET_ERROR; - if (subscription->is_raw()) { - ret = rcl_take_raw( - subscription->get_subscription_handle(), - reinterpret_cast(message.get()), &message_info); - } else { - ret = rcl_take( - subscription->get_subscription_handle(), - message.get(), &message_info); - } - if (ret == RCL_RET_OK) { - message_info.from_intra_process = false; - subscription->handle_message(message, message_info); - } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { + auto ret = rcl_take_raw( + subscription->get_subscription_handle().get(), + raw_msg.get(), &message_info); + if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string_safe()); + "rclcpp", + "take_raw failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } - subscription->return_message(message); + + // TODO(karsten1987): Can this be set to false directly after initialization? + message_info.from_intra_process = false; + + if (subscription->is_raw()) { + // TODO(karsten1987): Raise if opensplice !? + auto void_raw_msg = std::static_pointer_cast(raw_msg); + subscription->handle_message(void_raw_msg, message_info); + } else { + std::shared_ptr message = subscription->create_message(); + auto ts = subscription->get_message_type_support_handle(); + ret = rmw_deserialize(raw_msg.get(), &ts, message.get()); + if (ret == RCL_RET_OK) { + message_info.from_intra_process = false; + subscription->handle_message(message, message_info); + } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "could not deserialize raw message on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string_safe()); + rcl_reset_error(); + } + subscription->return_message(message); + } + + subscription->return_raw_message(raw_msg); } void diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 5241b7fe18..c32be81aeb 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -34,6 +34,7 @@ SubscriptionBase::SubscriptionBase( const rcl_subscription_options_t & subscription_options, bool is_raw) : node_handle_(node_handle), + type_support_(type_support_handle), is_raw_(is_raw) { std::weak_ptr weak_node_handle(node_handle_); @@ -114,6 +115,12 @@ SubscriptionBase::get_intra_process_subscription_handle() const return intra_process_subscription_handle_; } +rosidl_message_type_support_t +SubscriptionBase::get_message_type_support_handle() const +{ + return type_support_; +} + bool SubscriptionBase::is_raw() const { diff --git a/rclcpp/test/test_raw_message_allocator.cpp b/rclcpp/test/test_raw_message_allocator.cpp new file mode 100644 index 0000000000..ae94c651e3 --- /dev/null +++ b/rclcpp/test/test_raw_message_allocator.cpp @@ -0,0 +1,63 @@ +// Copyright 2018 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 "rcl/types.h" + +#include "test_msgs/msg/empty.hpp" + +TEST(TestRawMessageAllocator, default_allocator) { + using DummyMessageT = float; + auto mem_strategy = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + + auto msg0 = mem_strategy->borrow_raw_message(); + ASSERT_EQ(msg0->buffer_capacity, 0u); + mem_strategy->return_raw_message(msg0); + + auto msg100 = mem_strategy->borrow_raw_message(100); + ASSERT_EQ(msg100->buffer_capacity, 100u); + mem_strategy->return_raw_message(msg100); + + auto msg200 = mem_strategy->borrow_raw_message(); + rmw_raw_message_resize(msg200.get(), 200); + EXPECT_EQ(0u, msg200->buffer_length); + EXPECT_EQ(200u, msg200->buffer_capacity); + mem_strategy->return_raw_message(msg200); + + auto msg1000 = mem_strategy->borrow_raw_message(1000); + ASSERT_EQ(msg1000->buffer_capacity, 1000u); + rmw_raw_message_resize(msg1000.get(), 2000); + EXPECT_EQ(2000u, msg1000->buffer_capacity); + mem_strategy->return_raw_message(msg1000); +} + +TEST(TestRawMessageAllocator, borrow_from_subscription) { + // TODO(karsten1987): Check under Linux and Valgrind. MacOSX segfaults + rclcpp::init(0, NULL); + + auto node = std::make_shared("test_raw_message_allocator_node"); + std::shared_ptr sub = node->create_subscription("~/dummy_topic", [](std::shared_ptr test_msg){ (void) test_msg;}); + + auto msg0 = sub->create_raw_message(); + EXPECT_EQ(0u, msg0->buffer_capacity); + sub->return_raw_message(msg0); + + rclcpp::shutdown(); +} From b8e9d2854ba27582eb4c99aae85829721c3aba6a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 23 Apr 2018 18:14:31 -0700 Subject: [PATCH 10/17] linters --- .../include/rclcpp/message_memory_strategy.hpp | 4 ---- rclcpp/src/rclcpp/executor.cpp | 16 ++++++++-------- rclcpp/test/test_raw_message_allocator.cpp | 7 +++++-- rclcpp/test/test_time_source.cpp | 2 ++ 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 5d7046f6e5..6b7901347a 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -82,10 +82,6 @@ class MessageMemoryStrategy virtual std::shared_ptr borrow_raw_message(unsigned int capacity) { - // get rcutils_default allocator - // initialize raw message - // does this have to be a shared pointer ?! - //auto raw_msg = std::make_shared(rmw_get_zero_initialized_raw_message()); auto raw_msg = std::make_shared(); raw_msg->buffer_length = 0; raw_msg->buffer_capacity = 0; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 902eb4b202..e0034fdd9b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -285,13 +285,13 @@ Executor::execute_subscription( rmw_message_info_t message_info; auto ret = rcl_take_raw( - subscription->get_subscription_handle().get(), - raw_msg.get(), &message_info); + subscription->get_subscription_handle().get(), + raw_msg.get(), &message_info); if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take_raw failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string_safe()); + "rclcpp", + "take_raw failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } @@ -311,9 +311,9 @@ Executor::execute_subscription( subscription->handle_message(message, message_info); } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "could not deserialize raw message on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string_safe()); + "rclcpp", + "could not deserialize raw message on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } subscription->return_message(message); diff --git a/rclcpp/test/test_raw_message_allocator.cpp b/rclcpp/test/test_raw_message_allocator.cpp index ae94c651e3..1c15719026 100644 --- a/rclcpp/test/test_raw_message_allocator.cpp +++ b/rclcpp/test/test_raw_message_allocator.cpp @@ -25,7 +25,8 @@ TEST(TestRawMessageAllocator, default_allocator) { using DummyMessageT = float; - auto mem_strategy = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + auto mem_strategy = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); auto msg0 = mem_strategy->borrow_raw_message(); ASSERT_EQ(msg0->buffer_capacity, 0u); @@ -53,7 +54,9 @@ TEST(TestRawMessageAllocator, borrow_from_subscription) { rclcpp::init(0, NULL); auto node = std::make_shared("test_raw_message_allocator_node"); - std::shared_ptr sub = node->create_subscription("~/dummy_topic", [](std::shared_ptr test_msg){ (void) test_msg;}); + std::shared_ptr sub = + node->create_subscription("~/dummy_topic", []( + std::shared_ptr test_msg) {(void) test_msg;}); auto msg0 = sub->create_raw_message(); EXPECT_EQ(0u, msg0->buffer_capacity); diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index 0f80a2fa93..d6d4d0d497 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -280,6 +280,8 @@ TEST_F(TestTimeSource, callback_handler_erasure) { trigger_clock_changes(node); + fprintf(stderr, "trigger clock changes successful\n"); + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); From 9eb0a8288a7f55507244b657bf56dde29265f49e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 26 Apr 2018 18:37:07 -0700 Subject: [PATCH 11/17] explicit differentiation between take and take_raw --- rclcpp/src/rclcpp/executor.cpp | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index e0034fdd9b..461f91a76e 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -281,33 +281,32 @@ void Executor::execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription) { - auto raw_msg = subscription->create_raw_message(); rmw_message_info_t message_info; - - auto ret = rcl_take_raw( - subscription->get_subscription_handle().get(), - raw_msg.get(), &message_info); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take_raw failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string_safe()); - rcl_reset_error(); - } - // TODO(karsten1987): Can this be set to false directly after initialization? message_info.from_intra_process = false; if (subscription->is_raw()) { // TODO(karsten1987): Raise if opensplice !? + auto raw_msg = subscription->create_raw_message(); + auto ret = rcl_take_raw( + subscription->get_subscription_handle().get(), + raw_msg.get(), &message_info); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_raw failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string_safe()); + rcl_reset_error(); + } auto void_raw_msg = std::static_pointer_cast(raw_msg); subscription->handle_message(void_raw_msg, message_info); + subscription->return_raw_message(raw_msg); } else { std::shared_ptr message = subscription->create_message(); - auto ts = subscription->get_message_type_support_handle(); - ret = rmw_deserialize(raw_msg.get(), &ts, message.get()); + auto ret = rcl_take( + subscription->get_subscription_handle().get(), + message.get(), &message_info); if (ret == RCL_RET_OK) { - message_info.from_intra_process = false; subscription->handle_message(message, message_info); } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { RCUTILS_LOG_ERROR_NAMED( @@ -318,8 +317,6 @@ Executor::execute_subscription( } subscription->return_message(message); } - - subscription->return_raw_message(raw_msg); } void From c7e07026fe68d4aaf96d3308945836ebab33c596 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 1 May 2018 15:45:58 -0700 Subject: [PATCH 12/17] cleanup debug messages --- rclcpp/test/test_time_source.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index d6d4d0d497..0f80a2fa93 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -280,8 +280,6 @@ TEST_F(TestTimeSource, callback_handler_erasure) { trigger_clock_changes(node); - fprintf(stderr, "trigger clock changes successful\n"); - auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); From 7a957369c3ff4ecd6fe4f08bb46aa286aa3aad5e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 22 May 2018 11:25:54 -0700 Subject: [PATCH 13/17] rename to rmw_message_init` --- rclcpp/include/rclcpp/message_memory_strategy.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 6b7901347a..24282fcb3d 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -86,7 +86,7 @@ class MessageMemoryStrategy raw_msg->buffer_length = 0; raw_msg->buffer_capacity = 0; raw_msg->buffer = NULL; - rmw_initialize_raw_message(raw_msg.get(), 0, &rcutils_allocator_); + rmw_raw_message_init(raw_msg.get(), 0, &rcutils_allocator_); rmw_raw_message_resize(raw_msg.get(), capacity); return raw_msg; } From 44e195170d4d151dac082056077b3e34cb9faca7 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 23 May 2018 10:05:46 -0700 Subject: [PATCH 14/17] address comments --- .../rclcpp/message_memory_strategy.hpp | 13 ++++++------ rclcpp/include/rclcpp/publisher.hpp | 21 ++++++------------- rclcpp/src/rclcpp/executor.cpp | 11 +++++----- 3 files changed, 19 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 24282fcb3d..0fb1a469b8 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -82,12 +82,13 @@ class MessageMemoryStrategy virtual std::shared_ptr borrow_raw_message(unsigned int capacity) { - auto raw_msg = std::make_shared(); - raw_msg->buffer_length = 0; - raw_msg->buffer_capacity = 0; - raw_msg->buffer = NULL; - rmw_raw_message_init(raw_msg.get(), 0, &rcutils_allocator_); - rmw_raw_message_resize(raw_msg.get(), capacity); + auto raw_msg = std::shared_ptr(new rcl_message_raw_t, + [](rmw_message_raw_t * msg) { + rmw_raw_message_fini(msg); + delete msg; + }); + *raw_msg = rmw_get_zero_initialized_raw_message(); + rmw_raw_message_init(raw_msg.get(), capacity, &rcutils_allocator_); return raw_msg; } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index cf07b53dab..78ac7b83dd 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -205,11 +205,8 @@ class Publisher : public PublisherBase ipm.publisher_id = intra_process_publisher_id_; ipm.message_sequence = message_seq; auto status = rcl_publish(&intra_process_publisher_handle_, &ipm); - if (status != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to publish intra process message: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message"); } } else { // Always destroy the message, even if we don't consume it, for consistency. @@ -287,11 +284,8 @@ class Publisher : public PublisherBase throw std::runtime_error("storing raw messages in intra process is not supported yet."); } auto status = rcl_publish_raw(&publisher_handle_, raw_msg); - if (status != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to publish raw message: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish raw message"); } } @@ -305,11 +299,8 @@ class Publisher : public PublisherBase do_inter_process_publish(const MessageT * msg) { auto status = rcl_publish(&publisher_handle_, msg); - if (status != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to publish message: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message"); } } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 461f91a76e..078a07dfda 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -291,24 +291,25 @@ Executor::execute_subscription( auto ret = rcl_take_raw( subscription->get_subscription_handle().get(), raw_msg.get(), &message_info); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK == ret) { + auto void_raw_msg = std::static_pointer_cast(raw_msg); + subscription->handle_message(void_raw_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "take_raw failed for subscription on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } - auto void_raw_msg = std::static_pointer_cast(raw_msg); - subscription->handle_message(void_raw_msg, message_info); subscription->return_raw_message(raw_msg); } else { std::shared_ptr message = subscription->create_message(); auto ret = rcl_take( subscription->get_subscription_handle().get(), message.get(), &message_info); - if (ret == RCL_RET_OK) { + if (RCL_RET_OK == ret) { subscription->handle_message(message, message_info); - } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "could not deserialize raw message on topic '%s': %s", From 7666a1af82089fad1329563ddb62cbeffb36c27b Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 13 Jun 2018 16:12:54 +0200 Subject: [PATCH 15/17] address review comments --- .../rclcpp/message_memory_strategy.hpp | 19 ++++++++++++++----- rclcpp/include/rclcpp/publisher.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 2 -- rclcpp/src/rclcpp/subscription.cpp | 2 +- rclcpp/src/rclcpp/time_source.cpp | 3 ++- rclcpp/test/test_raw_message_allocator.cpp | 7 ++++--- 7 files changed, 23 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 0fb1a469b8..8234f647e8 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -21,6 +21,7 @@ #include "rcl/types.h" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -82,13 +83,22 @@ class MessageMemoryStrategy virtual std::shared_ptr borrow_raw_message(unsigned int capacity) { - auto raw_msg = std::shared_ptr(new rcl_message_raw_t, + auto msg = new rcl_message_raw_t; + *msg = rmw_get_zero_initialized_raw_message(); + auto ret = rmw_raw_message_init(msg, capacity, &rcutils_allocator_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + auto raw_msg = std::shared_ptr(msg, [](rmw_message_raw_t * msg) { - rmw_raw_message_fini(msg); + auto ret = rmw_raw_message_fini(msg); delete msg; + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "leaking memory"); + } }); - *raw_msg = rmw_get_zero_initialized_raw_message(); - rmw_raw_message_init(raw_msg.get(), capacity, &rcutils_allocator_); + return raw_msg; } @@ -111,7 +121,6 @@ class MessageMemoryStrategy virtual void return_raw_message(std::shared_ptr & raw_msg) { - rmw_raw_message_fini(raw_msg.get()); raw_msg.reset(); } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 78ac7b83dd..507529b781 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -280,7 +280,7 @@ class Publisher : public PublisherBase publish(const rcl_message_raw_t * raw_msg) { if (store_intra_process_message_) { - // not supported atm + // TODO(Karsten1987): support raw message passed by intraprocess throw std::runtime_error("storing raw messages in intra process is not supported yet."); } auto status = rcl_publish_raw(&publisher_handle_, raw_msg); diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index da9717cd2c..780f54baee 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -122,7 +122,7 @@ class SubscriptionBase rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; - rosidl_message_type_support_t + const rosidl_message_type_support_t & get_message_type_support_handle() const; bool diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 078a07dfda..de1f16b5bc 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -282,11 +282,9 @@ Executor::execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription) { rmw_message_info_t message_info; - // TODO(karsten1987): Can this be set to false directly after initialization? message_info.from_intra_process = false; if (subscription->is_raw()) { - // TODO(karsten1987): Raise if opensplice !? auto raw_msg = subscription->create_raw_message(); auto ret = rcl_take_raw( subscription->get_subscription_handle().get(), diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index c32be81aeb..510f101d88 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -115,7 +115,7 @@ SubscriptionBase::get_intra_process_subscription_handle() const return intra_process_subscription_handle_; } -rosidl_message_type_support_t +const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { return type_support_; diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index e492b64b34..b18b56c73b 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -75,7 +75,8 @@ void TimeSource::attachNode( auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); clock_subscription_ = rclcpp::create_subscription< - MessageT, decltype(cb), Alloc, MessageT, SubscriptionT>( + MessageT, decltype(cb), Alloc, MessageT, SubscriptionT + >( node_topics_.get(), topic_name, std::move(cb), diff --git a/rclcpp/test/test_raw_message_allocator.cpp b/rclcpp/test/test_raw_message_allocator.cpp index 1c15719026..ec65b1ce1c 100644 --- a/rclcpp/test/test_raw_message_allocator.cpp +++ b/rclcpp/test/test_raw_message_allocator.cpp @@ -37,20 +37,21 @@ TEST(TestRawMessageAllocator, default_allocator) { mem_strategy->return_raw_message(msg100); auto msg200 = mem_strategy->borrow_raw_message(); - rmw_raw_message_resize(msg200.get(), 200); + auto ret = rmw_raw_message_resize(msg200.get(), 200); + ASSERT_EQ(RCL_RET_OK, ret); EXPECT_EQ(0u, msg200->buffer_length); EXPECT_EQ(200u, msg200->buffer_capacity); mem_strategy->return_raw_message(msg200); auto msg1000 = mem_strategy->borrow_raw_message(1000); ASSERT_EQ(msg1000->buffer_capacity, 1000u); - rmw_raw_message_resize(msg1000.get(), 2000); + ret = rmw_raw_message_resize(msg1000.get(), 2000); + ASSERT_EQ(RCL_RET_OK, ret); EXPECT_EQ(2000u, msg1000->buffer_capacity); mem_strategy->return_raw_message(msg1000); } TEST(TestRawMessageAllocator, borrow_from_subscription) { - // TODO(karsten1987): Check under Linux and Valgrind. MacOSX segfaults rclcpp::init(0, NULL); auto node = std::make_shared("test_raw_message_allocator_node"); From 08d4f2bf554a049e59c513008775eb352cdf154f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 14 Jun 2018 02:07:12 +0200 Subject: [PATCH 16/17] raw->serialized --- rclcpp/CMakeLists.txt | 8 +- .../rclcpp/message_memory_strategy.hpp | 41 ++++----- rclcpp/include/rclcpp/publisher.hpp | 10 +-- rclcpp/include/rclcpp/subscription.hpp | 28 +++---- rclcpp/include/rclcpp/subscription_traits.hpp | 13 +-- rclcpp/src/rclcpp/executor.cpp | 18 ++-- rclcpp/src/rclcpp/subscription.cpp | 8 +- ... => test_serialized_message_allocator.cpp} | 30 +++---- rclcpp/test/test_subscription_traits.cpp | 84 +++++++++---------- 9 files changed, 121 insertions(+), 119 deletions(-) rename rclcpp/test/{test_raw_message_allocator.cpp => test_serialized_message_allocator.cpp} (63%) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 4eed7c372d..1bd484fcaf 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -260,13 +260,13 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() - ament_add_gtest(test_raw_message_allocator test/test_raw_message_allocator.cpp + ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}) - if(TARGET test_raw_message_allocator) - target_include_directories(test_raw_message_allocator PUBLIC + if(TARGET test_serialized_message_allocator) + target_include_directories(test_serialized_message_allocator PUBLIC ${test_msgs_INCLUDE_DIRS} ) - target_link_libraries(test_raw_message_allocator + target_link_libraries(test_serialized_message_allocator ${PROJECT_NAME} ${test_msgs_LIBRARIES} ) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 8234f647e8..53bddc0818 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -25,7 +25,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" -#include "rmw/raw_message.h" +#include "rmw/serialized_message.h" namespace rclcpp { @@ -44,9 +44,10 @@ class MessageMemoryStrategy using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = allocator::Deleter; - using RawMessageAllocTraits = allocator::AllocRebind; - using RawMessageAlloc = typename RawMessageAllocTraits::allocator_type; - using RawMessageDeleter = allocator::Deleter; + using SerializedMessageAllocTraits = allocator::AllocRebind; + using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type; + using SerializedMessageDeleter = + allocator::Deleter; using BufferAllocTraits = allocator::AllocRebind; using BufferAlloc = typename BufferAllocTraits::allocator_type; @@ -55,7 +56,7 @@ class MessageMemoryStrategy MessageMemoryStrategy() { message_allocator_ = std::make_shared(); - raw_message_allocator_ = std::make_shared(); + serialized_message_allocator_ = std::make_shared(); buffer_allocator_ = std::make_shared(); rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); } @@ -63,7 +64,7 @@ class MessageMemoryStrategy explicit MessageMemoryStrategy(std::shared_ptr allocator) { message_allocator_ = std::make_shared(*allocator.get()); - raw_message_allocator_ = std::make_shared(*allocator.get()); + serialized_message_allocator_ = std::make_shared(*allocator.get()); buffer_allocator_ = std::make_shared(*allocator.get()); rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); } @@ -81,30 +82,30 @@ class MessageMemoryStrategy return std::allocate_shared(*message_allocator_.get()); } - virtual std::shared_ptr borrow_raw_message(unsigned int capacity) + virtual std::shared_ptr borrow_serialized_message(unsigned int capacity) { - auto msg = new rcl_message_raw_t; - *msg = rmw_get_zero_initialized_raw_message(); - auto ret = rmw_raw_message_init(msg, capacity, &rcutils_allocator_); + auto msg = new rcl_serialized_message_t; + *msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret); } - auto raw_msg = std::shared_ptr(msg, - [](rmw_message_raw_t * msg) { - auto ret = rmw_raw_message_fini(msg); + auto serialized_msg = std::shared_ptr(msg, + [](rmw_serialized_message_t * msg) { + auto ret = rmw_serialized_message_fini(msg); delete msg; if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "leaking memory"); } }); - return raw_msg; + return serialized_msg; } - virtual std::shared_ptr borrow_raw_message() + virtual std::shared_ptr borrow_serialized_message() { - return borrow_raw_message(default_buffer_capacity_); + return borrow_serialized_message(default_buffer_capacity_); } virtual void set_default_buffer_capacity(unsigned int capacity) @@ -119,16 +120,16 @@ class MessageMemoryStrategy msg.reset(); } - virtual void return_raw_message(std::shared_ptr & raw_msg) + virtual void return_serialized_message(std::shared_ptr & serialized_msg) { - raw_msg.reset(); + serialized_msg.reset(); } std::shared_ptr message_allocator_; MessageDeleter message_deleter_; - std::shared_ptr raw_message_allocator_; - RawMessageDeleter raw_message_deleter_; + std::shared_ptr serialized_message_allocator_; + SerializedMessageDeleter serialized_message_deleter_; std::shared_ptr buffer_allocator_; BufferDeleter buffer_deleter_; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 507529b781..ec26790e75 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -277,15 +277,15 @@ class Publisher : public PublisherBase } void - publish(const rcl_message_raw_t * raw_msg) + publish(const rcl_serialized_message_t * serialized_msg) { if (store_intra_process_message_) { - // TODO(Karsten1987): support raw message passed by intraprocess - throw std::runtime_error("storing raw messages in intra process is not supported yet."); + // TODO(Karsten1987): support serialized message passed by intraprocess + throw std::runtime_error("storing serialized messages in intra process is not supported yet"); } - auto status = rcl_publish_raw(&publisher_handle_, raw_msg); + auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg); if (RCL_RET_OK != status) { - rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish raw message"); + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message"); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 780f54baee..e368550b64 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -66,7 +66,7 @@ class SubscriptionBase const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, - bool is_raw = false); + bool is_serialized = false); /// Default destructor. RCLCPP_PUBLIC @@ -94,10 +94,10 @@ class SubscriptionBase virtual std::shared_ptr create_message() = 0; - /// Borrow a new raw message - /** \return Shared pointer to a rcl_message_raw_t. */ - virtual std::shared_ptr - create_raw_message() = 0; + /// Borrow a new serialized message + /** \return Shared pointer to a rcl_message_serialized_t. */ + virtual std::shared_ptr + create_serialized_message() = 0; /// Check if we need to handle the message, and execute the callback if we do. /** @@ -112,10 +112,10 @@ class SubscriptionBase virtual void return_message(std::shared_ptr & message) = 0; - /// Return the message borrowed in create_raw_message. + /// Return the message borrowed in create_serialized_message. /** \param[in] message Shared pointer to the returned message. */ virtual void - return_raw_message(std::shared_ptr & message) = 0; + return_serialized_message(std::shared_ptr & message) = 0; virtual void handle_intra_process_message( @@ -126,7 +126,7 @@ class SubscriptionBase get_message_type_support_handle() const; bool - is_raw() const; + is_serialized() const; protected: std::shared_ptr intra_process_subscription_handle_; @@ -137,7 +137,7 @@ class SubscriptionBase RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - bool is_raw_; + bool is_serialized_; }; /// Subscription implementation, templated on the type of message this subscription receives. @@ -180,7 +180,7 @@ class Subscription : public SubscriptionBase ts, topic_name, subscription_options, - rclcpp::subscription_traits::is_raw_subscription_argument::value), + rclcpp::subscription_traits::is_serialized_subscription_argument::value), any_callback_(callback), message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), @@ -208,9 +208,9 @@ class Subscription : public SubscriptionBase return message_memory_strategy_->borrow_message(); } - std::shared_ptr create_raw_message() + std::shared_ptr create_serialized_message() { - return message_memory_strategy_->borrow_raw_message(); + return message_memory_strategy_->borrow_serialized_message(); } void handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) @@ -234,9 +234,9 @@ class Subscription : public SubscriptionBase message_memory_strategy_->return_message(typed_message); } - void return_raw_message(std::shared_ptr & message) + void return_serialized_message(std::shared_ptr & message) { - message_memory_strategy_->return_raw_message(message); + message_memory_strategy_->return_serialized_message(message); } void handle_intra_process_message( diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 8151f3ef60..3a73228f5e 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -33,24 +33,25 @@ namespace subscription_traits * Cheers! */ template -struct is_raw_subscription_argument : std::false_type +struct is_serialized_subscription_argument : std::false_type {}; template<> -struct is_raw_subscription_argument: std::true_type +struct is_serialized_subscription_argument: std::true_type {}; template<> -struct is_raw_subscription_argument>: std::true_type +struct is_serialized_subscription_argument> + : std::true_type {}; template -struct is_raw_subscription : is_raw_subscription_argument +struct is_serialized_subscription : is_serialized_subscription_argument {}; template -struct is_raw_callback - : is_raw_subscription_argument< +struct is_serialized_callback + : is_serialized_subscription_argument< typename rclcpp::function_traits::function_traits::template argument_type<0>> {}; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index de1f16b5bc..bdceed11c0 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -284,22 +284,22 @@ Executor::execute_subscription( rmw_message_info_t message_info; message_info.from_intra_process = false; - if (subscription->is_raw()) { - auto raw_msg = subscription->create_raw_message(); - auto ret = rcl_take_raw( + if (subscription->is_serialized()) { + auto serialized_msg = subscription->create_serialized_message(); + auto ret = rcl_take_serialized_message( subscription->get_subscription_handle().get(), - raw_msg.get(), &message_info); + serialized_msg.get(), &message_info); if (RCL_RET_OK == ret) { - auto void_raw_msg = std::static_pointer_cast(raw_msg); - subscription->handle_message(void_raw_msg, message_info); + auto void_serialized_msg = std::static_pointer_cast(serialized_msg); + subscription->handle_message(void_serialized_msg, message_info); } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", - "take_raw failed for subscription on topic '%s': %s", + "take_serialized failed for subscription on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } - subscription->return_raw_message(raw_msg); + subscription->return_serialized_message(serialized_msg); } else { std::shared_ptr message = subscription->create_message(); auto ret = rcl_take( @@ -310,7 +310,7 @@ Executor::execute_subscription( } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", - "could not deserialize raw message on topic '%s': %s", + "could not deserialize serialized message on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 510f101d88..f1448e1d7b 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -32,10 +32,10 @@ SubscriptionBase::SubscriptionBase( const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, - bool is_raw) + bool is_serialized) : node_handle_(node_handle), type_support_(type_support_handle), - is_raw_(is_raw) + is_serialized_(is_serialized) { std::weak_ptr weak_node_handle(node_handle_); auto custom_deletor = [weak_node_handle](rcl_subscription_t * rcl_subs) @@ -122,7 +122,7 @@ SubscriptionBase::get_message_type_support_handle() const } bool -SubscriptionBase::is_raw() const +SubscriptionBase::is_serialized() const { - return is_raw_; + return is_serialized_; } diff --git a/rclcpp/test/test_raw_message_allocator.cpp b/rclcpp/test/test_serialized_message_allocator.cpp similarity index 63% rename from rclcpp/test/test_raw_message_allocator.cpp rename to rclcpp/test/test_serialized_message_allocator.cpp index ec65b1ce1c..ae65f7f1eb 100644 --- a/rclcpp/test/test_raw_message_allocator.cpp +++ b/rclcpp/test/test_serialized_message_allocator.cpp @@ -23,45 +23,45 @@ #include "test_msgs/msg/empty.hpp" -TEST(TestRawMessageAllocator, default_allocator) { +TEST(TestSerializedMessageAllocator, default_allocator) { using DummyMessageT = float; auto mem_strategy = rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); - auto msg0 = mem_strategy->borrow_raw_message(); + auto msg0 = mem_strategy->borrow_serialized_message(); ASSERT_EQ(msg0->buffer_capacity, 0u); - mem_strategy->return_raw_message(msg0); + mem_strategy->return_serialized_message(msg0); - auto msg100 = mem_strategy->borrow_raw_message(100); + auto msg100 = mem_strategy->borrow_serialized_message(100); ASSERT_EQ(msg100->buffer_capacity, 100u); - mem_strategy->return_raw_message(msg100); + mem_strategy->return_serialized_message(msg100); - auto msg200 = mem_strategy->borrow_raw_message(); - auto ret = rmw_raw_message_resize(msg200.get(), 200); + auto msg200 = mem_strategy->borrow_serialized_message(); + auto ret = rmw_serialized_message_resize(msg200.get(), 200); ASSERT_EQ(RCL_RET_OK, ret); EXPECT_EQ(0u, msg200->buffer_length); EXPECT_EQ(200u, msg200->buffer_capacity); - mem_strategy->return_raw_message(msg200); + mem_strategy->return_serialized_message(msg200); - auto msg1000 = mem_strategy->borrow_raw_message(1000); + auto msg1000 = mem_strategy->borrow_serialized_message(1000); ASSERT_EQ(msg1000->buffer_capacity, 1000u); - ret = rmw_raw_message_resize(msg1000.get(), 2000); + ret = rmw_serialized_message_resize(msg1000.get(), 2000); ASSERT_EQ(RCL_RET_OK, ret); EXPECT_EQ(2000u, msg1000->buffer_capacity); - mem_strategy->return_raw_message(msg1000); + mem_strategy->return_serialized_message(msg1000); } -TEST(TestRawMessageAllocator, borrow_from_subscription) { +TEST(TestSerializedMessageAllocator, borrow_from_subscription) { rclcpp::init(0, NULL); - auto node = std::make_shared("test_raw_message_allocator_node"); + 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;}); - auto msg0 = sub->create_raw_message(); + auto msg0 = sub->create_serialized_message(); EXPECT_EQ(0u, msg0->buffer_capacity); - sub->return_raw_message(msg0); + sub->return_serialized_message(msg0); rclcpp::shutdown(); } diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index bc122ab074..3112cafffd 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -25,62 +25,62 @@ #include "test_msgs/msg/empty.hpp" -void raw_callback_copy(rcl_message_raw_t unused) +void serialized_callback_copy(rcl_serialized_message_t unused) { (void) unused; } -void raw_callback_shared_ptr(std::shared_ptr unused) +void serialized_callback_shared_ptr(std::shared_ptr unused) { (void) unused; } -void not_raw_callback(char * unused) +void not_serialized_callback(char * unused) { (void) unused; } -void not_raw_shared_ptr_callback(std::shared_ptr unused) +void not_serialized_shared_ptr_callback(std::shared_ptr unused) { (void) unused; } -void not_raw_unique_ptr_callback( +void not_serialized_unique_ptr_callback( test_msgs::msg::Empty::UniquePtrWithDeleter, test_msgs::msg::Empty>> unused) { (void) unused; } -TEST(TestSubscriptionTraits, is_raw_callback) { +TEST(TestSubscriptionTraits, is_serialized_callback) { // Test regular functions - auto cb1 = &raw_callback_copy; + auto cb1 = &serialized_callback_copy; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == true, - "rcl_message_raw_t in a first argument callback makes it a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == true, + "rcl_serialized_message_t in a first argument callback makes it a serialized callback"); - auto cb2 = &raw_callback_shared_ptr; + auto cb2 = &serialized_callback_shared_ptr; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == true, - "std::shared_ptr in a first argument callback makes it a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == true, + "std::shared_ptr in a callback makes it a serialized callback"); - auto cb3 = ¬_raw_callback; + auto cb3 = ¬_serialized_callback; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == false, - "passing a char * is not a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a char * is not a serialized callback"); - auto cb4 = ¬_raw_shared_ptr_callback; + auto cb4 = ¬_serialized_shared_ptr_callback; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == false, - "passing a std::shared_tr is not a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::shared_tr is not a serialized callback"); - auto cb5 = [](rcl_message_raw_t unused) -> void + auto cb5 = [](rcl_serialized_message_t unused) -> void { (void) unused; }; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == true, - "rcl_message_raw_t in a first argument callback makes it a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == true, + "rcl_serialized_message_t in a first argument callback makes it a serialized callback"); using MessageT = test_msgs::msg::Empty; using MessageTAllocator = std::allocator; @@ -90,13 +90,13 @@ TEST(TestSubscriptionTraits, is_raw_callback) { (void) unique_msg_ptr; }; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == false, - "passing a std::unique_ptr of test_msgs::msg::Empty is not a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::unique_ptr of test_msgs::msg::Empty is not a serialized callback"); - auto cb7 = ¬_raw_unique_ptr_callback; + auto cb7 = ¬_serialized_unique_ptr_callback; static_assert( - rclcpp::subscription_traits::is_raw_callback::value == false, - "passing a fancy unique_ptr of test_msgs::msg::Empty is not a raw callback"); + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback"); } TEST(TestSubscriptionTraits, callback_messages) { @@ -104,7 +104,7 @@ TEST(TestSubscriptionTraits, callback_messages) { std::is_same< std::shared_ptr, rclcpp::function_traits::function_traits< - decltype(not_raw_shared_ptr_callback) + decltype(not_serialized_shared_ptr_callback) >::template argument_type<0> >::value, "wrong!"); @@ -113,48 +113,48 @@ TEST(TestSubscriptionTraits, callback_messages) { char, rclcpp::subscription_traits::extract_message_type< rclcpp::function_traits::function_traits< - decltype(not_raw_shared_ptr_callback) + decltype(not_serialized_shared_ptr_callback) >::template argument_type<0> >::type >::value, "wrong!"); - auto cb1 = &raw_callback_copy; + auto cb1 = &serialized_callback_copy; static_assert( std::is_same< - rcl_message_raw_t, + rcl_serialized_message_t, rclcpp::subscription_traits::has_message_type::type>::value, - "raw callback message type is rcl_message_raw_t"); + "serialized callback message type is rcl_serialized_message_t"); - auto cb2 = &raw_callback_shared_ptr; + auto cb2 = &serialized_callback_shared_ptr; static_assert( std::is_same< - rcl_message_raw_t, + rcl_serialized_message_t, rclcpp::subscription_traits::has_message_type::type>::value, - "raw callback message type is rcl_message_raw_t"); + "serialized callback message type is rcl_serialized_message_t"); - auto cb3 = ¬_raw_callback; + auto cb3 = ¬_serialized_callback; static_assert( std::is_same< char *, rclcpp::subscription_traits::has_message_type::type>::value, - "not raw callback message type is char"); + "not serialized callback message type is char"); - auto cb4 = ¬_raw_shared_ptr_callback; + auto cb4 = ¬_serialized_shared_ptr_callback; static_assert( std::is_same< char, rclcpp::subscription_traits::has_message_type::type>::value, - "not raw shared_ptr callback message type is std::shared_ptr"); + "not serialized shared_ptr callback message type is std::shared_ptr"); - auto cb5 = [](rcl_message_raw_t unused) -> void + auto cb5 = [](rcl_serialized_message_t unused) -> void { (void) unused; }; static_assert( std::is_same< - rcl_message_raw_t, + rcl_serialized_message_t, rclcpp::subscription_traits::has_message_type::type>::value, - "raw callback message type is rcl_message_raw_t"); + "serialized callback message type is rcl_serialized_message_t"); using MessageT = test_msgs::msg::Empty; using MessageTAllocator = std::allocator; @@ -169,7 +169,7 @@ TEST(TestSubscriptionTraits, callback_messages) { rclcpp::subscription_traits::has_message_type::type>::value, "passing a std::unique_ptr of test_msgs::msg::Empty has message type Empty"); - auto cb7 = ¬_raw_unique_ptr_callback; + auto cb7 = ¬_serialized_unique_ptr_callback; static_assert( std::is_same< test_msgs::msg::Empty, From 36d2b9fc65f8584cfe1e613626633f174f752e61 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 14 Jun 2018 06:02:57 -0700 Subject: [PATCH 17/17] use size_t (#497) --- rclcpp/include/rclcpp/message_memory_strategy.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 53bddc0818..c4de703564 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -82,7 +82,7 @@ class MessageMemoryStrategy return std::allocate_shared(*message_allocator_.get()); } - virtual std::shared_ptr borrow_serialized_message(unsigned int capacity) + virtual std::shared_ptr borrow_serialized_message(size_t capacity) { auto msg = new rcl_serialized_message_t; *msg = rmw_get_zero_initialized_serialized_message(); @@ -108,7 +108,7 @@ class MessageMemoryStrategy return borrow_serialized_message(default_buffer_capacity_); } - virtual void set_default_buffer_capacity(unsigned int capacity) + virtual void set_default_buffer_capacity(size_t capacity) { default_buffer_capacity_ = capacity; } @@ -133,7 +133,7 @@ class MessageMemoryStrategy std::shared_ptr buffer_allocator_; BufferDeleter buffer_deleter_; - unsigned int default_buffer_capacity_ = 0; + size_t default_buffer_capacity_ = 0; rcutils_allocator_t rcutils_allocator_; };