From 1737d247eacdd49a40729525173efe2e7f8cb09c Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Fri, 9 Nov 2018 00:36:24 +0100 Subject: [PATCH] Implement converter plugin for CDR format and add converter plugins package (#48) * GH-111 Add package for converter plugins * GH-111 Add CDR converter plugin * GH-111 Add test for more primitives types * GH-116 Fix cdr converter after rebase on new converters interface * GH-116 Use rmw_serialize/rmw_deserialize directly in converter and link against rmw_fastrtps_cpp * Fix converter package.xml * Fix clang warnings * GH-30 Change interface to the same convention as rmw_(de)serialize * comply to new rcutils error handling API * use poco to load fastrtps * Update rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp Co-Authored-By: Karsten1987 --- ...rialization_format_converter_interface.hpp | 10 +- .../test/rosbag2/converter_test_plugin.cpp | 9 +- .../test/rosbag2/converter_test_plugin.hpp | 8 +- .../CMakeLists.txt | 81 +++++++ rosbag2_converter_default_plugins/package.xml | 31 +++ .../plugin_description.xml | 9 + .../cdr/cdr_converter.cpp | 131 +++++++++++ .../cdr/cdr_converter.hpp | 57 +++++ .../logging.hpp | 61 +++++ .../cdr/test_cdr_converter.cpp | 210 ++++++++++++++++++ .../src/rosbag2_storage/ros_helper.cpp | 4 +- .../rosbag2_test_common/memory_management.hpp | 30 ++- rosbag2_test_common/package.xml | 1 + .../generic_subscription.cpp | 2 +- .../src/rosbag2_transport/recorder.cpp | 2 +- .../src/rosbag2_transport/rosbag2_node.cpp | 2 +- 16 files changed, 626 insertions(+), 22 deletions(-) create mode 100644 rosbag2_converter_default_plugins/CMakeLists.txt create mode 100644 rosbag2_converter_default_plugins/package.xml create mode 100644 rosbag2_converter_default_plugins/plugin_description.xml create mode 100644 rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp create mode 100644 rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.hpp create mode 100644 rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/logging.hpp create mode 100644 rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp diff --git a/rosbag2/include/rosbag2/serialization_format_converter_interface.hpp b/rosbag2/include/rosbag2/serialization_format_converter_interface.hpp index 1dae21f317..cfdf26b889 100644 --- a/rosbag2/include/rosbag2/serialization_format_converter_interface.hpp +++ b/rosbag2/include/rosbag2/serialization_format_converter_interface.hpp @@ -33,14 +33,14 @@ class SerializationFormatConverterInterface virtual ~SerializationFormatConverterInterface() = default; virtual void deserialize( - std::shared_ptr ros_message, - std::shared_ptr serialized_message, - const rosidl_message_type_support_t * type_support) = 0; + std::shared_ptr serialized_message, + const rosidl_message_type_support_t * type_support, + std::shared_ptr ros_message) = 0; virtual void serialize( - std::shared_ptr serialized_message, std::shared_ptr ros_message, - const rosidl_message_type_support_t * type_support) = 0; + const rosidl_message_type_support_t * type_support, + std::shared_ptr serialized_message) = 0; }; } // namespace rosbag2 diff --git a/rosbag2/test/rosbag2/converter_test_plugin.cpp b/rosbag2/test/rosbag2/converter_test_plugin.cpp index d2cf7bfbb2..b10312aca0 100644 --- a/rosbag2/test/rosbag2/converter_test_plugin.cpp +++ b/rosbag2/test/rosbag2/converter_test_plugin.cpp @@ -19,10 +19,9 @@ #include "converter_test_plugin.hpp" void ConverterTestPlugin::deserialize( - - std::shared_ptr ros_message, const std::shared_ptr serialized_message, - const rosidl_message_type_support_t * type_support) + const rosidl_message_type_support_t * type_support, + std::shared_ptr ros_message) { (void) ros_message; (void) serialized_message; @@ -30,9 +29,9 @@ void ConverterTestPlugin::deserialize( } void ConverterTestPlugin::serialize( - std::shared_ptr serialized_message, const std::shared_ptr ros_message, - const rosidl_message_type_support_t * type_support) + const rosidl_message_type_support_t * type_support, + std::shared_ptr serialized_message) { (void) serialized_message; (void) ros_message; diff --git a/rosbag2/test/rosbag2/converter_test_plugin.hpp b/rosbag2/test/rosbag2/converter_test_plugin.hpp index 473111fc18..7cb0cdd601 100644 --- a/rosbag2/test/rosbag2/converter_test_plugin.hpp +++ b/rosbag2/test/rosbag2/converter_test_plugin.hpp @@ -25,14 +25,14 @@ class ConverterTestPlugin : public rosbag2::SerializationFormatConverterInterfac { public: void deserialize( - std::shared_ptr ros_message, std::shared_ptr serialized_message, - const rosidl_message_type_support_t * type_support) override; + const rosidl_message_type_support_t * type_support, + std::shared_ptr ros_message) override; void serialize( - std::shared_ptr serialized_message, std::shared_ptr ros_message, - const rosidl_message_type_support_t * type_support) override; + const rosidl_message_type_support_t * type_support, + std::shared_ptr serialized_message) override; }; #endif // ROSBAG2__CONVERTER_TEST_PLUGIN_HPP_ diff --git a/rosbag2_converter_default_plugins/CMakeLists.txt b/rosbag2_converter_default_plugins/CMakeLists.txt new file mode 100644 index 0000000000..64e5eb7f01 --- /dev/null +++ b/rosbag2_converter_default_plugins/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.5) +project(rosbag2_converter_default_plugins) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(poco_vendor REQUIRED) +find_package(Poco COMPONENTS Foundation) +find_package(pluginlib REQUIRED) +find_package(rosbag2 REQUIRED) +find_package(rosidl_generator_cpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp) + +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + pluginlib + Poco + rosbag2 + rcutils + rmw + rosidl_generator_cpp) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) + +pluginlib_export_plugin_description_file(rosbag2 plugin_description.xml) + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(rosbag2) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(rcutils REQUIRED) + find_package(rosbag2 REQUIRED) + find_package(rosbag2_test_common REQUIRED) + find_package(test_msgs REQUIRED) + + ament_lint_auto_find_test_dependencies() + + ament_add_gmock(test_cdr_converter + test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp + src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + if(TARGET test_cdr_converter) + ament_target_dependencies(test_cdr_converter + pluginlib + rosbag2 + rosbag2_test_common + rcutils + test_msgs) + endif() +endif() + +ament_package() diff --git a/rosbag2_converter_default_plugins/package.xml b/rosbag2_converter_default_plugins/package.xml new file mode 100644 index 0000000000..573e441313 --- /dev/null +++ b/rosbag2_converter_default_plugins/package.xml @@ -0,0 +1,31 @@ + + + + rosbag2_converter_default_plugins + 0.0.0 + Package containing default plugins for format converters + Karsten Knese + Apache License 2.0 + + ament_cmake + + ament_index_cpp + pluginlib + poco_vendor + rosidl_generator_cpp + rcutils + rmw + rosbag2 + + ament_cmake_gmock + ament_lint_auto + ament_lint_common + rcutils + rosbag2 + rosbag2_test_common + test_msgs + + + ament_cmake + + diff --git a/rosbag2_converter_default_plugins/plugin_description.xml b/rosbag2_converter_default_plugins/plugin_description.xml new file mode 100644 index 0000000000..63235c7ecf --- /dev/null +++ b/rosbag2_converter_default_plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + This is a converter plugin for cdr format. + + diff --git a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp new file mode 100644 index 0000000000..c9d6bffcdc --- /dev/null +++ b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp @@ -0,0 +1,131 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// 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 "cdr_converter.hpp" + +#include +#include + +#include "ament_index_cpp/get_resources.hpp" +#include "ament_index_cpp/get_package_prefix.hpp" + +#include "Poco/SharedLibrary.h" + +#include "rcutils/strdup.h" + +#include "../logging.hpp" + +namespace rosbag2_converter_default_plugins +{ + +inline +std::string get_package_library_path(const std::string & package_name) +{ + const char * filename_prefix; + const char * filename_extension; + const char * dynamic_library_folder; +#ifdef _WIN32 + filename_prefix = ""; + filename_extension = ".dll"; + dynamic_library_folder = "/bin/"; +#elif __APPLE__ + filename_prefix = "lib"; + filename_extension = ".dylib"; + dynamic_library_folder = "/lib/"; +#else + filename_prefix = "lib"; + filename_extension = ".so"; + dynamic_library_folder = "/lib/"; +#endif + + auto package_prefix = ament_index_cpp::get_package_prefix(package_name); + auto library_path = package_prefix + dynamic_library_folder + filename_prefix + + package_name + filename_extension; + + return library_path; +} + +CdrConverter::CdrConverter() +{ + auto library_path = get_package_library_path("rmw_fastrtps_dynamic_cpp"); + std::shared_ptr library; + try { + library = std::make_shared(library_path); + } catch (Poco::LibraryLoadException &) { + throw std::runtime_error( + std::string("poco exception: library could not be found:") + library_path); + } + + std::string serialize_symbol = "rmw_serialize"; + std::string deserialize_symbol = "rmw_deserialize"; + + if (!library->hasSymbol(serialize_symbol)) { + throw std::runtime_error( + std::string("poco exception: symbol not found: ") + serialize_symbol); + } + + if (!library->hasSymbol(deserialize_symbol)) { + throw std::runtime_error( + std::string("poco exception: symbol not found: ") + deserialize_symbol); + } + + serialize_fcn_ = (decltype(serialize_fcn_))library->getSymbol(serialize_symbol); + if (!serialize_fcn_) { + throw std::runtime_error( + std::string("poco exception: symbol of wrong type: ") + serialize_symbol); + } + + deserialize_fcn_ = (decltype(deserialize_fcn_))library->getSymbol(deserialize_symbol); + if (!deserialize_fcn_) { + throw std::runtime_error( + std::string("poco exception: symbol of wrong type: ") + deserialize_symbol); + } +} + +void CdrConverter::deserialize( + const std::shared_ptr serialized_message, + const rosidl_message_type_support_t * type_support, + std::shared_ptr ros_message) +{ + ros_message->topic_name = serialized_message->topic_name.c_str(); + ros_message->timestamp = serialized_message->time_stamp; + + printf("this should be a call to FASTRTPS\n"); + auto ret = + deserialize_fcn_(serialized_message->serialized_data.get(), type_support, ros_message->message); + if (ret != RMW_RET_OK) { + ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_ERROR("Failed to deserialize message."); + } +} + +void CdrConverter::serialize( + const std::shared_ptr ros_message, + const rosidl_message_type_support_t * type_support, + std::shared_ptr serialized_message) +{ + serialized_message->topic_name = std::string(ros_message->topic_name); + serialized_message->time_stamp = ros_message->timestamp; + + auto ret = serialize_fcn_( + ros_message->message, type_support, serialized_message->serialized_data.get()); + if (ret != RMW_RET_OK) { + ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_ERROR("Failed to serialize message."); + } +} + +} // namespace rosbag2_converter_default_plugins + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(rosbag2_converter_default_plugins::CdrConverter, + rosbag2::SerializationFormatConverterInterface) diff --git a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.hpp b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.hpp new file mode 100644 index 0000000000..60435e204a --- /dev/null +++ b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.hpp @@ -0,0 +1,57 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// 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 ROSBAG2_CONVERTER_DEFAULT_PLUGINS__CDR__CDR_CONVERTER_HPP_ +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS__CDR__CDR_CONVERTER_HPP_ + +#include +#include + +#include "rmw/types.h" + +#include "rosbag2/serialization_format_converter_interface.hpp" + +namespace rosbag2_converter_default_plugins +{ + +class CdrConverter : public rosbag2::SerializationFormatConverterInterface +{ +public: + CdrConverter(); + + void deserialize( + std::shared_ptr serialized_message, + const rosidl_message_type_support_t * type_support, + std::shared_ptr ros_message) override; + + void serialize( + std::shared_ptr ros_message, + const rosidl_message_type_support_t * type_support, + std::shared_ptr serialized_message) override; + +protected: + rmw_ret_t (* serialize_fcn_)( + const void *, + const rosidl_message_type_support_t *, + rmw_serialized_message_t *) = nullptr; + + rmw_ret_t (* deserialize_fcn_)( + const rmw_serialized_message_t *, + const rosidl_message_type_support_t *, + void *) = nullptr; +}; + +} // namespace rosbag2_converter_default_plugins + +#endif // ROSBAG2_CONVERTER_DEFAULT_PLUGINS__CDR__CDR_CONVERTER_HPP_ diff --git a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/logging.hpp b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/logging.hpp new file mode 100644 index 0000000000..a1ed5d409e --- /dev/null +++ b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/logging.hpp @@ -0,0 +1,61 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// 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 ROSBAG2_CONVERTER_DEFAULT_PLUGINS__LOGGING_HPP_ +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS__LOGGING_HPP_ + +#include +#include + +#include "rcutils/logging_macros.h" + +#define ROSBAG2_PACKAGE_NAME "rosbag2_converter_default_plugins" + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_INFO(...) \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_INFO_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_ERROR(...) \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_ERROR_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_WARN(...) \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_LOG_WARN_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_DEBUG(...) \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_CONVERTER_DEFAULT_PLUGINS_LOG_DEBUG_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#endif // ROSBAG2_CONVERTER_DEFAULT_PLUGINS__LOGGING_HPP_ diff --git a/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp b/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp new file mode 100644 index 0000000000..5c30add7eb --- /dev/null +++ b/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp @@ -0,0 +1,210 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// 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 "../../../src/rosbag2_converter_default_plugins/cdr/cdr_converter.hpp" +#include "../../../src/rosbag2_converter_default_plugins/logging.hpp" +#include "rcutils/strdup.h" +#include "rosbag2/serialization_format_converter_interface.hpp" +#include "rosbag2/typesupport_helpers.hpp" +#include "rosbag2_test_common/memory_management.hpp" +#include "test_msgs/message_fixtures.hpp" + +using rosbag2::SerializationFormatConverterInterface; +using namespace ::testing; // NOLINT +using namespace rosbag2_test_common; // NOLINT + +class CdrConverterTestFixture : public Test +{ +public: + CdrConverterTestFixture() + { + converter_ = std::make_unique(); + memory_management_ = std::make_unique(); + topic_name_ = "test_topic"; + allocator_ = rcutils_get_default_allocator(); + } + + std::shared_ptr make_shared_ros_message( + const std::string & topic_name = "") + { + auto ros_message = std::make_shared(); + ros_message->timestamp = 0; + ros_message->message = nullptr; + ros_message->allocator = allocator_; + + if (!topic_name.empty()) { + ros_message->topic_name = topic_name.c_str(); + } + + return ros_message; + } + + std::unique_ptr converter_; + std::unique_ptr memory_management_; + std::string topic_name_; + rcutils_allocator_t allocator_; +}; + +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_primitives) { + auto message = get_messages_primitives()[0]; + message->string_value = "test_deserialize"; + message->float64_value = 102.34; + message->int32_value = 10101010; + auto serialized_data = memory_management_->serialize_message(message); + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = serialized_data; + serialized_message->topic_name = topic_name_; + serialized_message->time_stamp = 1; + + auto ros_message = make_shared_ros_message(); + test_msgs::msg::Primitives primitive_test_msg; + ros_message->message = &primitive_test_msg; + auto type_support = rosbag2::get_typesupport("test_msgs/Primitives"); + + converter_->deserialize(serialized_message, type_support, ros_message); + + auto cast_message = static_cast(ros_message->message); + EXPECT_THAT(*cast_message, Eq(*message)); + EXPECT_THAT(ros_message->timestamp, Eq(serialized_message->time_stamp)); + EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); +} + +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_primitives) { + auto ros_message = make_shared_ros_message(topic_name_); + ros_message->timestamp = 1; + auto message = get_messages_primitives()[0]; + message->string_value = "test_serialize"; + message->float64_value = 102.34; + message->int32_value = 10101010; + ros_message->message = message.get(); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport("test_msgs/Primitives"); + + converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_->deserialize_message( + serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->timestamp)); +} + +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_static_array) { + auto message = get_messages_static_array_primitives()[0]; + message->string_values = {{"test_deserialize", "another string", "the third one"}}; + message->float64_values = {{102.34, 1.9, 1236.011}}; + message->int32_values = {{11, 36, 219}}; + auto serialized_data = memory_management_->serialize_message(message); + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = serialized_data; + serialized_message->topic_name = topic_name_; + serialized_message->time_stamp = 1; + + auto ros_message = make_shared_ros_message(); + test_msgs::msg::StaticArrayPrimitives primitive_test_msg; + ros_message->message = &primitive_test_msg; + auto type_support = rosbag2::get_typesupport("test_msgs/StaticArrayPrimitives"); + + converter_->deserialize(serialized_message, type_support, ros_message); + + auto cast_message = static_cast(ros_message->message); + EXPECT_THAT(*cast_message, Eq(*message)); + EXPECT_THAT(ros_message->timestamp, Eq(serialized_message->time_stamp)); + EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); +} + +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_static_array) { + auto ros_message = make_shared_ros_message(topic_name_); + ros_message->timestamp = 1; + auto message = get_messages_static_array_primitives()[0]; + message->string_values = {{"test_deserialize", "another string", "the third one"}}; + message->float64_values = {{102.34, 1.9, 1236.011}}; + message->int32_values = {{11, 36, 219}}; + ros_message->message = message.get(); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport("test_msgs/StaticArrayPrimitives"); + + converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_-> + deserialize_message(serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->timestamp)); +} + +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_dynamic_array_nest) { + auto message = get_messages_dynamic_array_nested()[0]; + test_msgs::msg::Primitives first_primitive_message; + first_primitive_message.string_value = "I am the first"; + first_primitive_message.float32_value = 35.7f; + test_msgs::msg::Primitives second_primitive_message; + second_primitive_message.string_value = "I am the second"; + second_primitive_message.float32_value = 135.72f; + message->primitive_values.push_back(first_primitive_message); + message->primitive_values.push_back(second_primitive_message); + auto serialized_data = memory_management_->serialize_message(message); + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = serialized_data; + serialized_message->topic_name = topic_name_; + serialized_message->time_stamp = 1; + + auto ros_message = make_shared_ros_message(); + test_msgs::msg::DynamicArrayNested dynamic_nested_message; + ros_message->message = &dynamic_nested_message; + auto type_support = rosbag2::get_typesupport("test_msgs/DynamicArrayNested"); + + converter_->deserialize(serialized_message, type_support, ros_message); + + auto cast_message = static_cast(ros_message->message); + EXPECT_THAT(*cast_message, Eq(*message)); + EXPECT_THAT(ros_message->timestamp, Eq(serialized_message->time_stamp)); + EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); +} + +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_dynamic_array_nest) { + auto ros_message = make_shared_ros_message(topic_name_); + ros_message->timestamp = 1; + auto message = get_messages_dynamic_array_nested()[0]; + test_msgs::msg::Primitives first_primitive_message; + first_primitive_message.string_value = "I am the first"; + first_primitive_message.float32_value = 35.7f; + test_msgs::msg::Primitives second_primitive_message; + second_primitive_message.string_value = "I am the second"; + second_primitive_message.float32_value = 135.72f; + message->primitive_values.push_back(first_primitive_message); + message->primitive_values.push_back(second_primitive_message); + ros_message->message = message.get(); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport("test_msgs/DynamicArrayNested"); + + converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_-> + deserialize_message(serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->timestamp)); +} diff --git a/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp b/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp index 9648f4896e..9c0c6c82e1 100644 --- a/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp +++ b/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp @@ -33,7 +33,7 @@ make_serialized_message(const void * data, size_t size) auto ret = rcutils_char_array_init(msg, size, &allocator); if (ret != RCUTILS_RET_OK) { throw std::runtime_error("Error allocating resources for serialized message: " + - std::string(rcutils_get_error_string_safe())); + std::string(rcutils_get_error_string().str)); } auto serialized_message = std::shared_ptr(msg, @@ -42,7 +42,7 @@ make_serialized_message(const void * data, size_t size) delete msg; if (error != RCUTILS_RET_OK) { ROSBAG2_STORAGE_LOG_ERROR_STREAM( - "Leaking memory. Error: " << rcutils_get_error_string_safe()); + "Leaking memory. Error: " << rcutils_get_error_string().str); } }); diff --git a/rosbag2_test_common/include/rosbag2_test_common/memory_management.hpp b/rosbag2_test_common/include/rosbag2_test_common/memory_management.hpp index a9baf6cabe..b7d49b88b4 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/memory_management.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/memory_management.hpp @@ -19,6 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rcutils/error_handling.h" namespace rosbag2_test_common { @@ -58,11 +59,34 @@ class MemoryManagement message.get()); if (error != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED("rosbag2_test_common", "Leaking memory. Error: %s", - rcutils_get_error_string_safe()); + rcutils_get_error_string().str); } return message; } + std::shared_ptr make_initialized_message() + { + auto msg = new rcutils_char_array_t; + *msg = rcutils_get_zero_initialized_char_array(); + auto ret = rcutils_char_array_init(msg, 0, &rcutils_allocator_); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("Error allocating resources for serialized message: " + + std::string(rcutils_get_error_string().str)); + } + + auto serialized_message = std::shared_ptr(msg, + [](rcutils_char_array_t * msg) { + int error = rcutils_char_array_fini(msg); + delete msg; + if (error != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED("rosbag2_test_common", + "Leaking memory. Error: %s", rcutils_get_error_string().str); + } + }); + + return serialized_message; + } + private: template inline @@ -79,7 +103,7 @@ class MemoryManagement auto ret = rcutils_char_array_init(msg, capacity, &rcutils_allocator_); if (ret != RCUTILS_RET_OK) { throw std::runtime_error("Error allocating resources for serialized message: " + - std::string(rcutils_get_error_string_safe())); + std::string(rcutils_get_error_string().str)); } auto serialized_message = std::shared_ptr(msg, @@ -88,7 +112,7 @@ class MemoryManagement delete msg; if (error != RCUTILS_RET_OK) { RCUTILS_LOG_ERROR_NAMED("rosbag2_test_common", "Leaking memory. Error: %s", - rcutils_get_error_string_safe()); + rcutils_get_error_string().str); } }); return serialized_message; diff --git a/rosbag2_test_common/package.xml b/rosbag2_test_common/package.xml index 42175aa7f4..638a0a188c 100644 --- a/rosbag2_test_common/package.xml +++ b/rosbag2_test_common/package.xml @@ -13,6 +13,7 @@ ament_lint_auto ament_lint_common rclcpp + rcutils rclcpp diff --git a/rosbag2_transport/src/rosbag2_transport/generic_subscription.cpp b/rosbag2_transport/src/rosbag2_transport/generic_subscription.cpp index aa49cdc2fc..dad146ad05 100644 --- a/rosbag2_transport/src/rosbag2_transport/generic_subscription.cpp +++ b/rosbag2_transport/src/rosbag2_transport/generic_subscription.cpp @@ -101,7 +101,7 @@ GenericSubscription::borrow_serialized_message(size_t capacity) delete msg; if (fini_return != RCL_RET_OK) { ROSBAG2_TRANSPORT_LOG_ERROR_STREAM( - "Failed to destroy serialized message: " << rcl_get_error_string_safe()); + "Failed to destroy serialized message: " << rcl_get_error_string().str); } }); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 7bbcef1b23..86aa35cc62 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -71,7 +71,7 @@ Recorder::create_subscription( int error = rcutils_system_time_now(&time_stamp); if (error != RCUTILS_RET_OK) { ROSBAG2_TRANSPORT_LOG_ERROR_STREAM( - "Error getting current time. Error:" << rcutils_get_error_string_safe()); + "Error getting current time. Error:" << rcutils_get_error_string().str); } bag_message->time_stamp = time_stamp; diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp index 17fec3c149..6d8f1b0a0d 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp @@ -109,7 +109,7 @@ std::string Rosbag2Node::expand_topic_name(const std::string & topic_name) if (ret != RCL_RET_OK) { ROSBAG2_TRANSPORT_LOG_ERROR_STREAM( "Failed to expand topic name " << topic_name << " with error: " << - rcutils_get_error_string_safe()); + rcutils_get_error_string().str); return ""; } std::string expanded_topic_name_std(expanded_topic_name);