From ba77cb5ca9ab6b80c9d06a8925aff6420bee2934 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 29 Apr 2019 18:08:10 -0300 Subject: [PATCH 1/4] Corrected publish calls with shared_ptr signature Signed-off-by: ivanpauno --- tlsf_cpp/example/allocator_example.cpp | 2 +- tlsf_cpp/test/test_tlsf.cpp | 29 -------------------------- 2 files changed, 1 insertion(+), 30 deletions(-) diff --git a/tlsf_cpp/example/allocator_example.cpp b/tlsf_cpp/example/allocator_example.cpp index 4797cc4..af757b3 100644 --- a/tlsf_cpp/example/allocator_example.cpp +++ b/tlsf_cpp/example/allocator_example.cpp @@ -110,7 +110,7 @@ int main(int argc, char ** argv) while (rclcpp::ok() && i < 100) { msg->data = i; ++i; - publisher->publish(msg); + publisher->publish(*msg); rclcpp::sleep_for(std::chrono::milliseconds(1)); executor.spin_some(); } diff --git a/tlsf_cpp/test/test_tlsf.cpp b/tlsf_cpp/test/test_tlsf.cpp index f78131f..1329a48 100644 --- a/tlsf_cpp/test/test_tlsf.cpp +++ b/tlsf_cpp/test/test_tlsf.cpp @@ -346,35 +346,6 @@ TEST_F(CLASSNAME(AllocatorTest, RMW_IMPLEMENTATION), type_traits_test) { "void unique ptr failed"); } -TEST_F(CLASSNAME(AllocatorTest, RMW_IMPLEMENTATION), allocator_shared_ptr) { - initialize(false, "allocator_shared_ptr"); - size_t counter = 0; - auto callback = [&counter](std_msgs::msg::UInt32::SharedPtr msg) -> void - { - EXPECT_EQ(counter, msg->data); - counter++; - }; - - rclcpp::subscription_traits::has_message_type::type a; - auto subscriber = node_->create_subscription( - "allocator_shared_ptr", callback, 10, subscription_options_, msg_memory_strategy_); - // Create msg to be published - auto msg = std::allocate_shared(*alloc.get()); - - rclcpp::sleep_for(std::chrono::milliseconds(1)); - // After test_initialization, global new should only be called from within TLSFAllocator. - test_init = true; - for (uint32_t i = 0; i < iterations; i++) { - msg->data = i; - publisher_->publish(msg); - rclcpp::sleep_for(std::chrono::milliseconds(1)); - executor_->spin_some(); - } - test_init = false; - EXPECT_FALSE(fail); - fail = false; -} - TEST_F(CLASSNAME(AllocatorTest, RMW_IMPLEMENTATION), allocator_unique_ptr) { initialize(true, "allocator_unique_ptr"); size_t counter = 0; From 08c6461821054802e05663d835124abd38a2654e Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Tue, 30 Apr 2019 17:22:48 -0300 Subject: [PATCH 2/4] Changed publish call with unique_ptr signature Signed-off-by: ivanpauno --- tlsf_cpp/test/test_tlsf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tlsf_cpp/test/test_tlsf.cpp b/tlsf_cpp/test/test_tlsf.cpp index 1329a48..de6a3e8 100644 --- a/tlsf_cpp/test/test_tlsf.cpp +++ b/tlsf_cpp/test/test_tlsf.cpp @@ -373,7 +373,7 @@ TEST_F(CLASSNAME(AllocatorTest, RMW_IMPLEMENTATION), allocator_unique_ptr) { auto msg = std::unique_ptr( std::allocator_traits::allocate(msg_alloc, 1)); msg->data = i; - publisher_->publish(msg); + publisher_->publish(std::move(msg)); rclcpp::sleep_for(std::chrono::milliseconds(1)); executor_->spin_some(); } From 7186777e89d8e7e69468b93289d07205ec2a3788 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Thu, 2 May 2019 13:26:52 -0300 Subject: [PATCH 3/4] Corrected linter failure Signed-off-by: ivanpauno --- tlsf_cpp/test/test_tlsf.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tlsf_cpp/test/test_tlsf.cpp b/tlsf_cpp/test/test_tlsf.cpp index de6a3e8..6c4ab16 100644 --- a/tlsf_cpp/test/test_tlsf.cpp +++ b/tlsf_cpp/test/test_tlsf.cpp @@ -15,6 +15,8 @@ #include #include #include +#include + #include "gtest/gtest.h" #ifdef __GNUC__ From c25a1f6c7625accb73d3a3f32d065af27be6f9ed Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Thu, 2 May 2019 13:27:20 -0300 Subject: [PATCH 4/4] Corrected allocator_examples Signed-off-by: ivanpauno --- tlsf_cpp/example/allocator_example.cpp | 32 ++++++++++++++++++-------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/tlsf_cpp/example/allocator_example.cpp b/tlsf_cpp/example/allocator_example.cpp index af757b3..b717b63 100644 --- a/tlsf_cpp/example/allocator_example.cpp +++ b/tlsf_cpp/example/allocator_example.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp/strategies/allocator_memory_strategy.hpp" @@ -27,6 +28,7 @@ using TLSFAllocator = tlsf_heap_allocator; int main(int argc, char ** argv) { using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; + using Alloc = TLSFAllocator; rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node; @@ -48,7 +50,7 @@ int main(int argc, char ** argv) auto context = rclcpp::contexts::default_context::get_global_default_context(); auto ipm_state = std::make_shared< - rclcpp::intra_process_manager::IntraProcessManagerImpl>>(); + rclcpp::intra_process_manager::IntraProcessManagerImpl>(); // Constructs the intra-process manager with a custom allocator. context->get_sub_context(ipm_state); @@ -73,24 +75,24 @@ int main(int argc, char ** argv) }; // Create a custom allocator and pass the allocator to the publisher and subscriber. - auto alloc = std::make_shared>(); - rclcpp::PublisherOptionsWithAllocator> publisher_options; + auto alloc = std::make_shared(); + rclcpp::PublisherOptionsWithAllocator publisher_options; publisher_options.allocator = alloc; auto publisher = node->create_publisher( "allocator_example", 10, publisher_options); - rclcpp::SubscriptionOptionsWithAllocator> subscription_options; + rclcpp::SubscriptionOptionsWithAllocator subscription_options; subscription_options.allocator = alloc; auto msg_mem_strat = std::make_shared< rclcpp::message_memory_strategy::MessageMemoryStrategy< - std_msgs::msg::UInt32, TLSFAllocator>>(alloc); + std_msgs::msg::UInt32, Alloc>>(alloc); auto subscriber = node->create_subscription( "allocator_example", callback, 10, subscription_options, msg_mem_strat); // Create a MemoryStrategy, which handles the allocations made by the Executor during the // execution path, and inject the MemoryStrategy into the Executor. std::shared_ptr memory_strategy = - std::make_shared>>(alloc); + std::make_shared>(alloc); rclcpp::executor::ExecutorArgs args; args.memory_strategy = memory_strategy; @@ -99,18 +101,28 @@ int main(int argc, char ** argv) // Add our node to the executor. executor.add_node(node); - // Create a message with the custom allocator, so that when the Executor deallocates the - // message on the execution path, it will use the custom deallocate. - auto msg = std::allocate_shared(*alloc.get()); + using MessageAllocTraits = + rclcpp::allocator::AllocRebind; + using MessageAlloc = MessageAllocTraits::allocator_type; + using MessageDeleter = rclcpp::allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + MessageDeleter message_deleter; + MessageAlloc message_alloc = *alloc; + rclcpp::allocator::set_allocator_for_deleter(&message_deleter, &message_alloc); rclcpp::sleep_for(std::chrono::milliseconds(1)); uint32_t i = 0; while (rclcpp::ok() && i < 100) { + // Create a message with the custom allocator, so that when the Executor deallocates the + // message on the execution path, it will use the custom deallocate. + auto ptr = MessageAllocTraits::allocate(message_alloc, 1); + MessageAllocTraits::construct(message_alloc, ptr); + MessageUniquePtr msg(ptr, message_deleter); msg->data = i; ++i; - publisher->publish(*msg); + publisher->publish(std::move(msg)); rclcpp::sleep_for(std::chrono::milliseconds(1)); executor.spin_some(); }