From 5fd47c4bfd1cce8bac020dd1905681dc01cce3c6 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 2 Apr 2024 22:40:20 -0700 Subject: [PATCH 1/2] use custom allocator from publisher option. Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/publisher.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index c6f16643c6..3e21f936b9 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -320,7 +320,7 @@ class Publisher : public PublisherBase { if (!intra_process_is_enabled_) { // In this case we're not using intra process. - auto ros_msg_ptr = std::make_unique(); + auto ros_msg_ptr = create_ros_message_unique_ptr(); rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); this->do_inter_process_publish(*ros_msg_ptr); return; @@ -330,7 +330,8 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto ros_msg_ptr = std::make_shared(); + auto ros_msg_ptr = std::allocate_shared< + ROSMessageType, ROSMessageTypeAllocator>(ros_message_type_allocator_); rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); this->do_intra_process_publish(std::move(msg)); this->do_inter_process_publish(*ros_msg_ptr); @@ -339,7 +340,8 @@ class Publisher : public PublisherBase } } else { if (buffer_) { - auto ros_msg_ptr = std::make_shared(); + auto ros_msg_ptr = std::allocate_shared< + ROSMessageType, ROSMessageTypeAllocator>(ros_message_type_allocator_); rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); buffer_->add_shared(ros_msg_ptr); } @@ -367,7 +369,7 @@ class Publisher : public PublisherBase { if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. - auto ros_msg_ptr = std::make_unique(); + auto ros_msg_ptr = create_ros_message_unique_ptr(); rclcpp::TypeAdapter::convert_to_ros_message(msg, *ros_msg_ptr); this->do_inter_process_publish(*ros_msg_ptr); return; From a71ec71fda1dcf6bc2182d540542b672e2278466 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 15 Apr 2024 10:55:40 -0700 Subject: [PATCH 2/2] std::memcpy only available size of destination buffer. Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index d70b0e382d..313f8ea411 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -118,8 +118,8 @@ struct TypeAdapter const custom_type & source, ros_message_type & destination) { - destination.size = source.size(); - std::memcpy(destination.data.data(), source.data(), source.size()); + destination.size = std::min(source.size(), destination.data.max_size()); + std::memcpy(destination.data.data(), source.data(), destination.size); } static void