From 51a4d2ea39af94bf55b224d7b76a05650d5bc038 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 5 Mar 2024 17:53:55 -0500 Subject: [PATCH] Fix TypeAdapted publishing with large messages. (#2443) Mostly by ensuring we aren't attempting to store large messages on the stack. Also add in tests. I verified that before these changes, the tests failed, while after them they succeed. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/publisher.hpp | 24 +++---- rclcpp/test/msg/LargeMessage.msg | 3 + rclcpp/test/rclcpp/CMakeLists.txt | 1 + .../test_publisher_with_type_adapter.cpp | 71 ++++++++++++++++++- 4 files changed, 84 insertions(+), 15 deletions(-) create mode 100644 rclcpp/test/msg/LargeMessage.msg diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 08a98bdf1a..c6f16643c6 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -290,8 +290,8 @@ class Publisher : public PublisherBase { // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg); + this->do_inter_process_publish(msg); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. @@ -318,12 +318,12 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // In this case we're not using intra process. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); - return this->do_inter_process_publish(ros_msg); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } bool inter_process_publish_needed = @@ -331,9 +331,6 @@ class Publisher : public PublisherBase if (inter_process_publish_needed) { auto ros_msg_ptr = std::make_shared(); - // TODO(clalancette): This is unnecessarily doing an additional conversion - // that may have already been done in do_intra_process_publish_and_return_shared(). - // We should just reuse that effort. 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); @@ -368,13 +365,12 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // Avoid double allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); - // In this case we're not using intra process. - return this->do_inter_process_publish(ros_msg); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. diff --git a/rclcpp/test/msg/LargeMessage.msg b/rclcpp/test/msg/LargeMessage.msg new file mode 100644 index 0000000000..1e383c0bae --- /dev/null +++ b/rclcpp/test/msg/LargeMessage.msg @@ -0,0 +1,3 @@ +# A message with a size larger than the default Linux stack size +uint8[10485760] data +uint64 size diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c17ed6fe06..25ca4082e1 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -6,6 +6,7 @@ add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}") rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/Header.msg + ../msg/LargeMessage.msg ../msg/MessageWithHeader.msg ../msg/String.msg DEPENDENCIES builtin_interfaces diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index e96e1e3a38..9367a89014 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include +#include #include #include #include @@ -25,6 +25,7 @@ #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/msg/large_message.hpp" #include "rclcpp/msg/string.hpp" @@ -105,6 +106,32 @@ struct TypeAdapter } }; +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::LargeMessage; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.size = source.size(); + std::memcpy(destination.data.data(), source.data(), source.size()); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination.resize(source.size); + std::memcpy(destination.data(), source.data.data(), source.size); + } +}; + } // namespace rclcpp /* @@ -336,3 +363,45 @@ TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { assert_message_was_received(); } } + +TEST_F(TestPublisher, test_large_message_unique) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + auto message_data = std::make_unique(); + message_data->reserve(length); + std::fill(message_data->begin(), message_data->begin() + length, '#'); + pub->publish(std::move(message_data)); +} + +TEST_F(TestPublisher, test_large_message_constref) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + std::string message_data; + message_data.reserve(length); + std::fill(message_data.begin(), message_data.begin() + length, '#'); + pub->publish(message_data); +}